bs_constraints

bs_constraints

The bs_constraints package contains all cost functions, and fuse constraint implementations used in beam_slam

Relative Pose

Relative pose constraints are any constraints that compute an error on the relative change between two states, meaning that the initial pose estimates are not constrained but the relationship between two poses is.

Motion

Motion constraints are relative pose constraints produced by motion models, which act to bridge any asynchronous states. In beam_slam only a Unicycle3d motion model is implemented, however more complex models can be implemented to suit your robot.

Global

Global constraints can also be thought of as any absolute pose constraints, which constrains the pose itself using the given covariance.

Visual

We implement 3 different reprojection errors, all of which are operating on distorted pixels (no undistorting):

1. Vanilla reprojection with a normalizing covariance weighting matrix (strictly pixel to pixel distance)
2. Unit Sphere reprojection which is similar to the cost function used in VINS-MONO, where the distance is computed in the unit sphere rather than the image plane
3. VILENS reprojection residual which computes a normal pixel to pixel distance, and is then weighted by a covariance matrix that is computed directly with the camera models distortion (i.e. pixels further to the edge with more distortion have a larger covariance)