Merge pull request #1816 from truher/team100_camera_resectioning

add example CameraResectioning.py

python/gtsam/examples/CameraResectioning.py

@@ -0,0 +1,85 @@
+# pylint: disable=consider-using-from-import,invalid-name,no-name-in-module,no-member,missing-function-docstring
+"""
+This is a 1:1 transcription of CameraResectioning.cpp.
+"""
+import numpy as np
+from gtsam import Cal3_S2, CustomFactor, LevenbergMarquardtOptimizer, KeyVector
+from gtsam import NonlinearFactor, NonlinearFactorGraph
+from gtsam import PinholeCameraCal3_S2, Point2, Point3, Pose3, Rot3, Values
+from gtsam.noiseModel import Base as SharedNoiseModel, Diagonal
+from gtsam.symbol_shorthand import X
+
+
+def resectioning_factor(
+    model: SharedNoiseModel,
+    key: int,
+    calib: Cal3_S2,
+    p: Point2,
+    P: Point3,
+) -> NonlinearFactor:
+
+    def error_func(this: CustomFactor, v: Values, H: list[np.ndarray]) -> np.ndarray:
+        pose = v.atPose3(this.keys()[0])
+        camera = PinholeCameraCal3_S2(pose, calib)
+        if H is None:
+            return camera.project(P) - p
+        Dpose = np.zeros((2, 6), order="F")
+        Dpoint = np.zeros((2, 3), order="F")
+        Dcal = np.zeros((2, 5), order="F")
+        result = camera.project(P, Dpose, Dpoint, Dcal) - p
+        H[0] = Dpose
+        return result
+
+    return CustomFactor(model, KeyVector([key]), error_func)
+
+
+def main() -> None:
+    """
+    Camera: f = 1, Image: 100x100, center: 50, 50.0
+    Pose (ground truth): (Xw, -Yw, -Zw, [0,0,2.0]')
+    Known landmarks:
+       3D Points: (10,10,0) (-10,10,0) (-10,-10,0) (10,-10,0)
+    Perfect measurements:
+       2D Point:  (55,45)   (45,45)    (45,55)     (55,55)
+    """
+
+    # read camera intrinsic parameters
+    calib = Cal3_S2(1, 1, 0, 50, 50)
+
+    # 1. create graph
+    graph = NonlinearFactorGraph()
+
+    # 2. add factors to the graph
+    measurement_noise = Diagonal.Sigmas(np.array([0.5, 0.5]))
+    graph.add(
+        resectioning_factor(
+            measurement_noise, X(1), calib, Point2(55, 45), Point3(10, 10, 0)
+        )
+    )
+    graph.add(
+        resectioning_factor(
+            measurement_noise, X(1), calib, Point2(45, 45), Point3(-10, 10, 0)
+        )
+    )
+    graph.add(
+        resectioning_factor(
+            measurement_noise, X(1), calib, Point2(45, 55), Point3(-10, -10, 0)
+        )
+    )
+    graph.add(
+        resectioning_factor(
+            measurement_noise, X(1), calib, Point2(55, 55), Point3(10, -10, 0)
+        )
+    )
+
+    # 3. Create an initial estimate for the camera pose
+    initial: Values = Values()
+    initial.insert(X(1), Pose3(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(0, 0, 1)))
+
+    # 4. Optimize the graph using Levenberg-Marquardt
+    result: Values = LevenbergMarquardtOptimizer(graph, initial).optimize()
+    result.print("Final result:\n")
+
+
+if __name__ == "__main__":
+    main()