Python
import unittest
import numpy as np
import cv2

class MathSolvePnpTest(unittest.TestCase):
    def test_MathSolvePnpRansacTest(self):
        coordinates3d = np.array([
            [-2970.04, 22.7401, -295.871], [-2965.97, 251.251, -299.211], [-2961.9, 479.762, -302.551],
            [-2967.7, 21.5298, -375.828], [-2963.63, 250.041, -379.168], [-2959.56, 478.551, -382.508],
            [-2965.37, 20.3194, -455.785], [-2961.3, 248.83, -459.125], [-2957.23, 477.341, -462.465],
            [-3781.22, 90.6926, -364.541], [-3778.94, 319.251, -365.375], [-3776.65, 547.81, -366.208],
            [-3779.74, 90.3861, -444.527], [-3777.45, 318.945, -445.361], [-3775.17, 547.503, -446.194],
            [-3778.25, 90.0796, -524.513], [-3775.97, 318.638, -525.346], [-3773.69, 547.197, -526.18]
        ], dtype=np.float32)

        coordinates2d = np.array([
            [33.095, 45.9702], [365.301, 45.7652], [696.981, 44.6757],
            [37.2094, 170.679], [367.98, 171.518], [696.879, 166.68],
            [38.6706, 290.908], [367.531, 287.723], [696.276, 284.076],
            [136.725, 109.046], [404.561, 106.04], [668.244, 98.8107],
            [139.404, 206.921], [406.535, 202.8], [669.206, 193.597],
            [140.827, 300.732], [406.881, 294.125], [668.616, 288.088]
        ], dtype=np.float32)
        
        if False:
            coordinates3d = np.array([
                [-2970.04, 22.7401, -295.871], [-2965.97, 251.251, -299.211],
                [-2963.63, 250.041, -379.168],
                [-3781.22, 90.6926, -364.541],
                [-3775.97, 318.638, -525.346]
            ], dtype=np.float32)
    
            coordinates2d = np.array([
                [33.095, 45.9702], [365.301, 45.7652],
                [367.98, 171.518],
                [136.725, 109.046],
                [406.881, 294.125]
            ], dtype=np.float32)

        intrinsicMatrix = np.array([
            [4815.14, 0.0, 420],
            [0.0, 5321.18, 210],
            [0.0, 0.0, 1.0]
        ], dtype=np.float32)

        extrinsicsExpected = np.array([
            [0.0566549, 0.998194, -0.0199624, -126.085],
            [0.0558855, -0.0231338, -0.998169, -233.8],
            [-0.996828, 0.0554356, -0.0570952, 356.557],
            [0.0, 0.0, 0.0, 1.0]
        ], dtype=np.float32)

        reprojectionErrorLimit = 4.0

        distortionCoefficients = np.zeros((4, 1))  # Assuming no distortion

        # Solve PnP RANSAC
        ransac = False
        if ransac:
            success, rvec, tvec, inliers = cv2.solvePnPRansac(
                coordinates3d, coordinates2d, intrinsicMatrix, distortionCoefficients, 
                None, None, False, 500, reprojectionErrorLimit, 0.995, None, 
                cv2.SOLVEPNP_EPNP
            )
        if not ransac:
            success, rvec, tvec = cv2.solvePnP(
                coordinates3d, coordinates2d, intrinsicMatrix, distortionCoefficients, 
                flags=cv2.SOLVEPNP_EPNP
            )

        self.assertTrue(success)

        # Convert rotation vector to rotation matrix
        rotation_matrix, _ = cv2.Rodrigues(rvec)
        extrinsics = np.hstack((rotation_matrix, tvec))
        extrinsics = np.vstack((extrinsics, [0, 0, 0, 1]))
        
        # Print the resulting pose
        np.set_printoptions(precision=5, suppress=True)
        print("Extrinsics (Pose):")
        print(extrinsics)
        print(inliers)

        # Compare the extrinsics
        for rowIndex in range(3):
            for columnIndex in range(3):
                diff = abs(extrinsics[rowIndex, columnIndex] - extrinsicsExpected[rowIndex, columnIndex])
                self.assertLess(diff, 0.0001)

if __name__ == '__main__':
    unittest.main()
Extrinsics (Pose):
[[   0.05666    0.99819   -0.01996 -126.0847 ]
 [   0.05589   -0.02313   -0.99817 -233.79996]
 [  -0.99683    0.05544   -0.0571   356.55722]
 [   0.         0.         0.         1.     ]]

E
======================================================================
ERROR: test_MathSolvePnpRansacTest (__main__.MathSolvePnpTest.test_MathSolvePnpRansacTest)
----------------------------------------------------------------------
Traceback (most recent call last):
  File "script.py", line 82, in test_MathSolvePnpRansacTest
    print(inliers)
          ^^^^^^^
UnboundLocalError: cannot access local variable 'inliers' where it is not associated with a value

----------------------------------------------------------------------
Ran 1 test in 0.002s

FAILED (errors=1)
line 91, in <module>
    unittest.main()
  File "/lib/python312.zip/unittest/main.py", line 105, in __init__
    self.runTests()
  File "/lib/python312.zip/unittest/main.py", line 288, in runTests
    sys.exit(1)
SystemExit: 1