Add Python test harness for openCVHelp class (#1557)

photon-lib/py/photonlibpy/estimation/rotTrlTransform3d.py

@@ -24,9 +24,32 @@ class RotTrlTransform3d:
     def getRotation(self) -> Rotation3d:
         return self.rot
 
-    def apply(self, trlToApply: Translation3d) -> Translation3d:
+    def applyTranslation(self, trlToApply: Translation3d) -> Translation3d:
         return trlToApply.rotateBy(self.rot) + self.trl
 
+    def applyRotation(self, rotToApply: Rotation3d) -> Rotation3d:
+        return rotToApply + self.rot
+
+    def applyPose(self, poseToApply: Pose3d) -> Pose3d:
+        return Pose3d(
+            self.applyTranslation(poseToApply.translation()),
+            self.applyRotation(poseToApply.rotation()),
+        )
+
+    def applyTrls(self, rots: list[Rotation3d]) -> list[Rotation3d]:
+        retVal: list[Rotation3d] = []
+        for rot in rots:
+            retVal.append(self.applyRotation(rot))
+        return retVal
+
     @classmethod
     def makeRelativeTo(cls, pose: Pose3d) -> Self:
         return cls(pose.rotation(), pose.translation()).inverse()
+
+    @classmethod
+    def makeBetweenPoses(cls, initial: Pose3d, last: Pose3d) -> Self:
+        return cls(
+            last.rotation() - initial.rotation(),
+            last.translation()
+            - initial.translation().rotateBy(last.rotation() - initial.rotation()),
+        )