GTSAM factor for FRC

truher · December 24, 2024, 10:48pm

For folks experimenting with GTSAM for pose estimation:

I contributed some factors to the project, and I’d like your feedback.

One of them, “PlanarProjectionFactor1,” estimates Pose2d given constant landmarks, camera calibration and offset. The use case here is “smoothing,” e.g. using one of the Fixed Lag Smoother classes. Another, “PlanarProjectionFactor3,” uses constant landmarks to estimate pose, calibration, and offset simultaneously – the use case here is automatic camera calibration. There’s also an SFM factor, but since our landmarks are fixed, it’s not as useful.

Anyway, I’d like these to be generally useful to FRC, so please comment on this PR!

github.com/borglab/gtsam

projection and SFM for 2d poses

borglab:develop ← truher:team100_frc_factors

opened 12:34AM - 16 Dec 24 UTC

truher

+784 -0

These are the "robot on the floor" versions of SFM and Projection factors, patte…rned after the similarly-named pose3 versions. These are intended for localization in high school robotics, where the landmarks are AprilTag markers in known locations. PlanarProjectionFactor uses constant Pose3 landmarks, a constant camera Pose3 offset, and a constant camera Cal3DS2 calibration. The measurement is camera pixels. The free variable is a Pose2 representing the position of the robot on the floor, and the expectation is that this factor will be used for real-time smoothing. PlanarSFMFactor uses the same robot Pose2 variable, constant Pose3 landmarks and camera pixel measurement, as above, but instead of using constant camera Pose3 offset and Cal3DS2 calibration, these are also variables. This factor is primarily intended for calibration of camera offset and distortion. Oh, one important thing, the "zero" camera offset is such that the camera bore is parallel to the "zero" pose2 (i.e. facing +x), which I think may be different than the normal body-offset zero. Using the same zero as the pose2 seems less confusing to clients (who expect a zero offset camera to be looking at pose2 zero), but it might be more confusing to folks who are more familiar with the gtsam camera.

thatmattguy · December 26, 2024, 2:07am

Lit, thanks! Good to see that the maintainers are excited about FRC (mom get the camera we famous)

calcmogul · December 26, 2024, 2:12am

Pushing FRC controls tech forward, one PR at a time.

truher · December 26, 2024, 3:27am

Prof Dellaert has been incredibly patient and helpful.

viggy96 · December 26, 2024, 10:58am

Can someone ELI5 this to me?

SLAB-Mr.Thomas · December 26, 2024, 2:13pm

A 5 year old might be too advanced for me as the technology has pushed the entry age down to age 3 years (and I saw one game for 2 year olds). Ages 3 to 5 robot navigation programming

truher · December 26, 2024, 6:19pm

GTSAM implements the “Factor Graph” approach to estimation. The general idea is to decompose the global estimation problem into a number of local terms (“factors”) representing residuals (e.g. camera re-projection error), and then iteratively solve the resulting (sparse) nonlinear system. If the errors are Gaussian, then this is a maximum-likelihood estimator.

A nice property of this estimator is that it optimizes over the factor residuals in measurement space, e.g. camera pixels. (Today we specify a pose-isotropic uncertainty of the camera-derived pose estimate, which is definitely wrong.)

GTSAM includes many factors useful in the robotics domain, e.g. representing odometry, or the bearing to a target, etc, and also variable types like 2d and 3d poses, cameras with distortion, etc. Much of this system is aimed at more-difficult problems than we encounter in FRC, e.g. airborne SLAM. The system is written in C++ with Matlab and python wrappers: we use python on Raspberry Pi.

The specific thing I made is a factor that represents the camera residual for 2d poses but 3d targets, which happened not to have been written before.