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 estimateinverse_range_prior (
float
) – The mean of the inverse range priorweight (
float
) – The weight of the priorsigma (
float
) – The standard deviation of the priorepsilon (
float
) – Small positive value
- Outputs:
res: 1dof residual of the prior
- Return type:
- 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 camerasource_calibration_storage (
Matrix
) – The storage vector of the source camera calibrationtarget_pose (
Pose3
) – The pose of the target cameratarget_calibration_storage (
Matrix
) – The storage vector of the target camera calibrationsource_inverse_range (
float
) – The inverse range of the landmark in the source camerasource_pixel (
Matrix21
) – The location of the landmark in the source cameratarget_pixel (
Matrix21
) – The location of the correspondence in the target cameraepsilon (
float
) – Small positive valuecamera_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 camerasource_calibration_storage (
Matrix
) – The storage vector of the source camera calibrationtarget_pose (
Pose3
) – The pose of the target cameratarget_calibration_storage (
Matrix
) – The storage vector of the target camera calibrationsource_inverse_range (
float
) – The inverse range of the landmark in the source camerasource_pixel (
Matrix21
) – The location of the landmark in the source cameratarget_pixel (
Matrix21
) – The location of the correspondence in the target cameraweight (
float
) – The weight of the factorgnc_mu (
float
) – The mu convexity parameter for the Barron noise modelgnc_scale (
float
) – The scale parameter for the Barron noise modelepsilon (
float
) – Small positive valuecamera_model_class (
Type
[CameraCal
]) – The subclass of CameraCal to use as the camera model
- Outputs:
res: 2dof residual of the reprojection
- Return type:
- 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 cameratarget_pose (
Pose3
) – The pose of the target cameratarget_calibration_storage (
Matrix
) – The storage vector of the target camera calibrationsource_inverse_range (
float
) – The inverse range of the landmark in the source camerap_camera_source (
Matrix31
) – The location of the landmark in the source camera coordinate, will be normalizedtarget_pixel (
Matrix21
) – The location of the correspondence in the target cameraepsilon (
float
) – Small positive valuetarget_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 cameratarget_pose (
Pose3
) – The pose of the target cameratarget_calibration_storage (
Matrix
) – The storage vector of the target spherical camera calibrationsource_inverse_range (
float
) – The inverse range of the landmark in the source camerap_camera_source (
Matrix31
) – The location of the landmark in the source camera coordinate, will be normalizedtarget_pixel (
Matrix21
) – The location of the correspondence in the target cameraweight (
float
) – The weight of the factorgnc_mu (
float
) – The mu convexity parameter for the Barron noise modelgnc_scale (
float
) – The scale parameter for the Barron noise modelepsilon (
float
) – Small positive valuetarget_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:
- generate(output_dir, config=None)[source]¶
Generate the SLAM package for the given language.
- Parameters:
output_dir (
Path
) – Directory to generate outputs intoconfig (
Optional
[CodegenConfig
]) – CodegenConfig, defaults to the default C++ config
- Return type:
None