Pose Estimation with Limelight #36
Conversation
| } | ||
|
|
||
| fun Pose3d.closerToInTranslation(pose1: Pose3d, pose2: Pose3d): Pose3d { | ||
| val distToPose1 = |
There was a problem hiding this comment.
(this - pose1).translation.norm?
There was a problem hiding this comment.
or (this.translation - pose1.translation).norm
| return Pose3d(this.x, this.y, 0.0.meters, Rotation3d(0.0.degrees, 0.0.degrees, this.rotation)) | ||
| } | ||
|
|
||
| fun Angle.rotateBy(angle: Angle): Angle { |
There was a problem hiding this comment.
what is the difference between this and just adding two angles?
| cameraTransform: Transform3d, | ||
| targetHeight: Length | ||
| ): Pose3d { | ||
| val horizontalAngleFromRobot = tx + cameraTransform.rotation.x |
There was a problem hiding this comment.
I think it is cleaner to do all of the math from the camera's perspective then rotate by the camera transform in the end.
Also, tx and rotation about x are not describing the same thing (tx is rotation about Z I think because it is describing how far in the horizontal FOV the blob is)
There was a problem hiding this comment.
also make sure your sign convention is what WPILib uses -- is limelight tx positive in the direction you expect?
| val rotationFromTargetToRobot = | ||
| Rotation3d(0.0.degrees, verticalAngleFromRobot, horizontalAngleFromRobot) | ||
|
|
||
| val xDistanceFromTargetToRobot = (targetHeight - cameraTransform.x) / verticalAngleFromRobot.tan |
|
|
||
| return currentPose | ||
| .toPose3d() | ||
| .transformBy(cameraTransform) |
There was a problem hiding this comment.
why do this if we already ended up in the robot coordinate frame earlier?
is it literally just an if statement this feels stupidly easy
No description provided.