search

SciKit-Surgery / scikit-surgerycalibration

scikit-surgery-calibration provides algorithms designed for the calibration of surgical instruments
http://scikit-surgery.github.io/scikit-surgery/
Other
7 stars 2 forks source link

Remove dependendcy on scikit-surgeryopencvcpp #46

Closed thompson318 closed 1 year ago

thompson318 commented 1 year ago

It looks like we only use scikit-surgeryopencvcpp here https://github.com/SciKit-Surgery/scikit-surgerycalibration/blob/master/sksurgerycalibration/video/video_calibration_metrics.py#L174. We should try and remove this dependency. i.e. re-implment triangulate_points_using_hartley locally

mxochicale commented 1 year ago

Thanks Steve to point out where triangulated=cvpy.triangulate_points_using_hartley( is used in video_calibration_metrics.py, implemented: https://github.com/SciKit-Surgery/scikit-surgeryopencvcpp/blob/master/Code/Lib/sksTriangulate.cpp#L291 and tested https://github.com/SciKit-Surgery/scikit-surgeryopencvcpp/blob/master/Testing/sksTriangulateTest.cpp

Next steps

Notes

mxochicale commented 1 year ago

Tutorials https://scikit-surgerycalibration.readthedocs.io/en/latest/tutorials/calibration.html seems to work much better using jupypter notebook, specially for this bit: "If you hold the chessboard in view of your camera, and run the below cell repeatedly, it will grab some frames." But got few errors:

ValueError: not enough values to unpack (expected 3, got 2)

 Sorted with `proj_err, params = calibrator.calibrate()`

* [x] for Dot Calibration, I get: error: OpenCV(4.6.0) :-1: error: (-5:Bad argument) in function 'circle'

error Traceback (most recent call last) /tmp/ipykernel_44007/3599310274.py in 9 # Draw detected points on frame 10 for point in img_pts: ---> 11 frame = cv2.circle(frame, (point[0][0], point[0][1]), 5, (255, 0 ,0), -1) 12 13 print(f"Detected {number_of_points} points")

error: OpenCV(4.6.0) :-1: error: (-5:Bad argument) in function 'circle'

Overload resolution failed:

  • Can't parse 'center'. Sequence item with index 0 has a wrong type
  • Can't parse 'center'. Sequence item with index 0 has a wrong type

## conda env packages

Some useful commands to manage your conda env:

LIST CONDA ENVS: conda list -n *VE # show list of installed packages

UPDATE CONDA: conda update -n base -c defaults conda

INSTALL CONDA EV: conda env create -f *VE.yml

UPDATE CONDA ENV: conda env update -f *VE.yml --prune

ACTIVATE CONDA ENV: conda activate *VE

REMOVE CONDA ENV: conda remove -n *VE --all

name: scikit-surgerycalibrationVE channels:

mxochicale commented 1 year ago

Few errors from the C++ to python migration: 

Sorted with   `P1 = np.zeros((3, 4, 1), dtype=np.double)` > https://numpy.org/doc/stable/user/basics.types.html

* [x] `AttributeError: module 'cv2' has no attribute 'Point3d'`

Traceback (most recent call last): File "sand.py", line 5, in u1 = cv2.Point3d AttributeError: module 'cv2' has no attribute 'Point3d'

sorted with `u1 = np.zeros((3, 1, 1), dtype=np.double)` > https://docs.opencv.org/4.6.0/df/d6c/classcv_1_1Point3__.html

* [x] `AttributeError: 'numpy.ndarray' object has no attribute 'row'`

Traceback (most recent call last): File "sand.py", line 76, in p2x1 = np.zeros(P1.row[2] * X, dtype=np.double)[0] AttributeError: 'numpy.ndarray' object has no attribute 'row'

sorted with `P1[2][:]`

* [x] `double p2x1 = cv::Mat_<double>(cv::Mat_<double>(P1).row(2)*X)(0);` to its python version

Traceback (most recent call last): File "sand.py", line 80, in p2x1 = np.zeros(P1[2][:] * X, dtype=np.double)[0] TypeError: only integer scalar arrays can be converted to a scalar index


`row` seems to be implemented in an old version of opencv https://docs.opencv.org/2.4/modules/core/doc/basic_structures.html#mat-row
mxochicale commented 1 year ago

Do you know the reference of the following snippet at cv::Point3d InternalIterativeTriangulatePointUsingSVD(...? would to see the equation for this: double p2x1 = cv::Mat_<double>(cv::Mat_<double>(P1).row(2)*X)(0);

https://github.com/SciKit-Surgery/scikit-surgeryopencvcpp/blob/6c2748afa8e3ac54677a8922bf755548d9a9bbf6/Code/Lib/sksTriangulate.cpp#L263-L279

thompson318 commented 1 year ago

Do we want p2x1=(P1[2][:] * X)[0] ?

mxochicale commented 1 year ago

Using np.arrays did not correctly make matrix multiplications, so I use asmatrix and transpose T 

    print(f'i: {i}')
    AA = np.asmatrix( P1[2][:] ).T

    print(f'PRINT: { AA.shape }')
    print(f'PRINT: {AA} ')

    BB = np.asmatrix( X ).T
    print(f'PRINT: { BB.shape }')
    print(f'PRINT: { BB }')

    print(f'PRINT AA*BB: { (AA * BB).shape }')
    print(f'PRINT AA*BB: { (AA * BB) }')

resulting in

i: 0
PRINT: (1, 4)
PRINT: [[0. 0. 0. 0.]] 
PRINT: (4, 1)
PRINT: [[0.]
 [0.]
 [0.]
 [1.]]
PRINT AA*BB: (1, 1)
PRINT AA*BB: [[0.]]

The following make sense to me but need to check the equation to confirm its right implementation

p2x1 = (np.asmatrix(P1[2][:]).T) * (np.asmatrix(X).T)
mxochicale commented 1 year ago

Looking for further references of p2x1 and p2x1 in mitkCameraCalibrationFacade.cxx and sksTriangulate.cpp

    double p2x1 = cv::Mat_<double>(cv::Mat_<double>(P1).row(2)*X)(0);
    double p2x2 = cv::Mat_<double>(cv::Mat_<double>(P2).row(2)*X)(0);

def IterativeLinearLSTriangulation(u, P, u1, P1):
  wi = 1
  wi1 = 1
  X = np.empty([4,1]);
  for i in xrange(0, 10):
    X_ = LinearLSTriangulation(u,P,u1,P1)
    X[0] = X_[0] 
    X[1] = X_[1] 
    X[2] = X_[2] 
    X[3] = 1.0

    p2x = (P[2,].dot(X))[0]
    p2x1 = (P1[2,].dot(X))[0]

    if (abs(wi - p2x) <= EPSILON and abs(wi1 - p2x1) <= EPSILON):
      break

    wi = p2x
    wi1 = p2x1

    A = np.array([[(u[0]*P[2,0]-P[0,0])/wi, (u[0]*P[2,1]-P[0,1])/wi, (u[0]*P[2,2]-P[0,2])/wi],
                  [(u[1]*P[2,0]-P[1,0])/wi, (u[1]*P[2,1]-P[1,1])/wi, (u[1]*P[2,2]-P[1,2])/wi],
                  [(u1[0]*P1[2,0]-P1[0,0])/wi1, (u1[0]*P1[2,1]-P1[0,1])/wi1, (u1[0]*P1[2,2]-P1[0,2])/wi1],
                  [(u1[1]*P1[2,0]-P1[1,0])/wi1, (u1[1]*P1[2,1]-P1[1,1])/wi1, (u1[1]*P1[2,2]-P1[1,2])/wi1]])

    B = np.array([[-(u[0]*P[2,3]-P[0,3])/wi],
                  [-(u[1]*P[2,3]-P[1,3])/wi],
                  [-(u1[0]*P1[2,3]-P1[0,3])/wi1],
                  [-(u1[1]*P1[2,3]-P1[1,3])/wi1]])

    (retval, X_) = cv2.solve(A,B,flags=cv2.DECOMP_SVD)

    X[0] = X_[0]
    X[1] = X_[1]
    X[2] = X_[2] 
    X[3] = 1.0

  return X
def iterative_LS_triangulation(u1, P1, u2, P2, tolerance=3.e-5):
    """
    Iterative (Linear) Least Squares based triangulation.
    From "Triangulation", Hartley, R.I. and Sturm, P., Computer vision and image understanding, 1997.
    Relative speed: 0.025

    (u1, P1) is the reference pair containing normalized image coordinates (x, y) and the corresponding camera matrix.
    (u2, P2) is the second pair.
    "tolerance" is the depth convergence tolerance.

    Additionally returns a status-vector to indicate outliers:
        1: inlier, and in front of both cameras
        0: outlier, but in front of both cameras
        -1: only in front of second camera
        -2: only in front of first camera
        -3: not in front of any camera
    Outliers are selected based on non-convergence of depth, and on negativity of depths (=> behind camera(s)).

    u1 and u2 are matrices: amount of points equals #rows and should be equal for u1 and u2.
    """
    A = np.zeros((4, 3))
    b = np.zeros((4, 1))

    # Create array of triangulated points
    x = np.empty((4, len(u1))); x[3, :].fill(1)    # create empty array of homogenous 3D coordinates
    x_status = np.empty(len(u1), dtype=int)

    # Initialize C matrices
    C1 = np.array(iterative_LS_triangulation_C)
    C2 = np.array(iterative_LS_triangulation_C)

    for xi in range(len(u1)):
        # Build C matrices, to construct A and b in a concise way
        C1[:, 2] = u1[xi, :]
        C2[:, 2] = u2[xi, :]

        # Build A matrix
        A[0:2, :] = C1.dot(P1[0:3, 0:3])    # C1 * R1
        A[2:4, :] = C2.dot(P2[0:3, 0:3])    # C2 * R2

        # Build b vector
        b[0:2, :] = C1.dot(P1[0:3, 3:4])    # C1 * t1
        b[2:4, :] = C2.dot(P2[0:3, 3:4])    # C2 * t2
        b *= -1

        # Init depths
        d1 = d2 = 1.

        for i in range(10):    # Hartley suggests 10 iterations at most
            # Solve for x vector
            #x_old = np.array(x[0:3, xi])    # TODO: remove
            cv2.solve(A, b, x[0:3, xi:xi+1], cv2.DECOMP_SVD)

            # Calculate new depths
            d1_new = P1[2, :].dot(x[:, xi])
            d2_new = P2[2, :].dot(x[:, xi])

            # Convergence criterium
            #print i, d1_new - d1, d2_new - d2, (d1_new > 0 and d2_new > 0)    # TODO: remove
            #print i, (d1_new - d1) / d1, (d2_new - d2) / d2, (d1_new > 0 and d2_new > 0)    # TODO: remove
            #print i, np.sqrt(np.sum((x[0:3, xi] - x_old)**2)), (d1_new > 0 and d2_new > 0)    # TODO: remove
            ##print i, u1[xi, :] - P1[0:2, :].dot(x[:, xi]) / d1_new, u2[xi, :] - P2[0:2, :].dot(x[:, xi]) / d2_new    # TODO: remove
            #print bool(i) and ((d1_new - d1) / (d1 - d_old), (d2_new - d2) / (d2 - d1_old), (d1_new > 0 and d2_new > 0))    # TODO: remove
            ##if abs(d1_new - d1) <= tolerance and abs(d2_new - d2) <= tolerance: print "Orig cond met"    # TODO: remove
            if abs(d1_new - d1) <= tolerance and \
                    abs(d2_new - d2) <= tolerance:
            #if i and np.sum((x[0:3, xi] - x_old)**2) <= 0.0001**2:
            #if abs((d1_new - d1) / d1) <= 3.e-6 and \
                    #abs((d2_new - d2) / d2) <= 3.e-6: #and \
                    #abs(d1_new - d1) <= tolerance and \
                    #abs(d2_new - d2) <= tolerance:
            #if i and 1 - abs((d1_new - d1) / (d1 - d_old)) <= 1.e-2 and \    # TODO: remove
                    #1 - abs((d2_new - d2) / (d2 - d1_old)) <= 1.e-2 and \    # TODO: remove
                    #abs(d1_new - d1) <= tolerance and \    # TODO: remove
                    #abs(d2_new - d2) <= tolerance:    # TODO: remove
                break

            # Re-weight A matrix and b vector with the new depths
            A[0:2, :] *= 1 / d1_new
            A[2:4, :] *= 1 / d2_new
            b[0:2, :] *= 1 / d1_new
            b[2:4, :] *= 1 / d2_new

            # Update depths
            #d_old = d1    # TODO: remove
            #d1_old = d2    # TODO: remove
            d1 = d1_new
            d2 = d2_new

        # Set status
        x_status[xi] = ( i < 10 and                       # points should have converged by now
                         (d1_new > 0 and d2_new > 0) )    # points should be in front of both cameras
        if d1_new <= 0: x_status[xi] -= 1
        if d2_new <= 0: x_status[xi] -= 2

    return x[0:3, :].T.astype(output_dtype), x_status
mxochicale commented 1 year ago

Just testing few matrix operations

    print(f'P1= {P1}')
    print(f'P1[2,] = {P1[2,]}')
    print(f'P1[2][:] = {P1[2][:]}')

logs

P1= [[[0.]
  [0.]
  [0.]
  [0.]]

 [[0.]
  [0.]
  [0.]
  [0.]]

 [[0.]
  [0.]
  [0.]
  [0.]]]
P1[2,] = [[0.]
 [0.]
 [0.]
 [0.]]
P1[2][:] = [[0.]
 [0.]
 [0.]
 [0.]]
mxochicale commented 1 year ago

Steve and I discussed current status and next steps to unit test:

def triangulate_points_using_hartley_opencv(left_undistorted,
                                            right_undistorted,
                                            leftCameraIntrinsicParams,
                                            rightCameraIntrinsicParams,
                                            leftToRightRotationMatrix,
                                            leftToRightTranslationVector):

Steve suggest to look into points_in_2d to extract left_undistorted and right_undistorted :

points_in_2d = np.zeros((4, 4), dtype=np.double) points_in_2d[0, 0] = 1100.16 points_in_2d[0, 1] = 262.974 points_in_2d[0, 2] = 1184.84 points_in_2d[0, 3] = 241.915 points_in_2d[1, 0] = 1757.74 points_in_2d[1, 1] = 228.971 points_in_2d[1, 2] = 1843.52 points_in_2d[1, 3] = 204.083 points_in_2d[2, 0] = 1065.44 points_in_2d[2, 1] = 651.593 points_in_2d[2, 2] = 1142.75 points_in_2d[2, 3] = 632.817 points_in_2d[3, 0] = 1788.22 points_in_2d[3, 1] = 650.41 points_in_2d[3, 2] = 1867.78 points_in_2d[3, 3] = 632.59

from: scikit-surgeryopencvcpp/blob/master/Testing/sksTriangulateTest.cpp

cv::Mat pointsIn2D = cv::Mat::zeros(4, 4, CV_64FC1); pointsIn2D.at(0, 0) = 1100.16; pointsIn2D.at(0, 1) = 262.974; pointsIn2D.at(0, 2) = 1184.84; pointsIn2D.at(0, 3) = 241.915; pointsIn2D.at(1, 0) = 1757.74; pointsIn2D.at(1, 1) = 228.971; pointsIn2D.at(1, 2) = 1843.52; pointsIn2D.at(1, 3) = 204.083; pointsIn2D.at(2, 0) = 1065.44; pointsIn2D.at(2, 1) = 651.593; pointsIn2D.at(2, 2) = 1142.75; pointsIn2D.at(2, 3) = 632.817; pointsIn2D.at(3, 0) = 1788.22; pointsIn2D.at(3, 1) = 650.41; pointsIn2D.at(3, 2) = 1867.78; pointsIn2D.at(3, 3) = 632.59;

mxochicale commented 1 year ago 
Terminal logs:

tests/algorithms/test_triangulate.py::test_triangulate_points_using_hartley_opencv [[2.01218631e+03 0.00000000e+00 9.44717371e+02 0.00000000e+00] [0.00000000e+00 2.01796602e+03 6.17109398e+02 0.00000000e+00] [0.00000000e+00 0.00000000e+00 1.00000000e+00 0.00000000e+00]]

[[ 2.00989229e+03 -2.32425602e+01 1.10225114e+03 -8.06867616e+03] [-1.53917938e+01 2.03922444e+03 5.93705918e+02 1.26399541e+03] [-2.53880000e-02 -2.24050000e-02 9.99426000e-01 1.30025600e+00]]

[[1100.16 262.974] [1757.74 228.971] [1065.44 651.593] [1788.22 650.41 ]]

[[1184.84 241.915] [1843.52 204.083] [1142.75 632.817] [1867.78 632.59 ]] FAILED

    triangulated_cv = cv2.triangulatePoints(p_l,
                                            p_r,
                                            left_undistorted,
                                          right_undistorted)

E cv2.error: OpenCV(4.5.5) /io/opencv/modules/calib3d/src/triangulate.cpp:75: error: (-209:Sizes of input arguments do not match) Number of proj points coordinates must be == 2 in function 'icvTriangulatePoints'

sksurgerycalibration/algorithms/triangulate.py:283: error


* Sorted out by transposing vectors:
print(f'\n {p_l}')
print(f'\n {p_r}')
print(f'\n {left_undistorted.T}')
print(f'\n {right_undistorted.T}')
triangulated_cv = cv2.triangulatePoints(p_l,
                                        p_r,
                                        left_undistorted.T,
                                        right_undistorted.T)
logs:

tests/algorithms/test_triangulate.py::test_triangulate_points_using_hartley_opencv [[2.01218631e+03 0.00000000e+00 9.44717371e+02 0.00000000e+00] [0.00000000e+00 2.01796602e+03 6.17109398e+02 0.00000000e+00] [0.00000000e+00 0.00000000e+00 1.00000000e+00 0.00000000e+00]]

[[ 2.00989229e+03 -2.32425602e+01 1.10225114e+03 -8.06867616e+03] [-1.53917938e+01 2.03922444e+03 5.93705918e+02 1.26399541e+03] [-2.53880000e-02 -2.24050000e-02 9.99426000e-01 1.30025600e+00]]

[[1100.16 1757.74 1065.44 1788.22 ] [ 262.974 228.971 651.593 650.41 ]]

[[1184.84 1843.52 1142.75 1867.78 ] [ 241.915 204.083 632.817 632.59 ]] PASSED

mxochicale commented 1 year ago

Currently migrating the following test to its python version https://github.com/SciKit-Surgery/scikit-surgeryopencvcpp/blob/master/Testing/sksTriangulateTest.cpp , along with ComputeRMSBetweenCorrespondingPoints() from https://github.com/SciKit-Surgery/scikit-surgeryopencvcpp/blob/master/Code/Lib/sksMaths.cpp

  cv::Mat modelPointsTransposed;
  cv::transpose(modelPoints, modelPointsTransposed);

  cv::Mat rotatedModelPoints = cv::Mat(modelPointsTransposed.rows, modelPointsTransposed.cols, CV_64FC1);
  cv::gemm(leftRotation, modelPointsTransposed, 1, cv::Mat(), 0, rotatedModelPoints);

  cv::Mat modelPointsRotatedAndTransposed;
  cv::transpose(rotatedModelPoints, modelPointsRotatedAndTransposed);

  cv::Mat transformedModelPoints = cv::Mat(modelPointsRotatedAndTransposed.rows, modelPointsRotatedAndTransposed.cols, CV_64FC1);
  for (int r = 0; r < modelPointsRotatedAndTransposed.rows; r++)
  {
    for (int c = 0; c < modelPointsRotatedAndTransposed.cols; c++)
    {
      transformedModelPoints.at<double>(r, c) = modelPointsRotatedAndTransposed.at<double>(r, c) + leftTranslation.at<double>(c, 0);
    }
  }

  std::cerr << "pointsFromHartley=" << pointsFromHartley << std::endl;
  std::cerr << "pointsFromMidpoint=" << pointsFromMidpoint << std::endl;
  std::cerr << "transformedModelPoints=" << transformedModelPoints << std::endl;

  double rmsHartley = sks::ComputeRMSBetweenCorrespondingPoints(transformedModelPoints, pointsFromHartley);
  double rmsMidpoint = sks::ComputeRMSBetweenCorrespondingPoints(transformedModelPoints, pointsFromMidpoint);

  std::cerr << "rmsHartley=" << rmsHartley << std::endl;
  std::cerr << "rmsMidpoints=" << rmsMidpoint << std::endl;

Notes

mxochicale commented 1 year ago

pylint logs for Module 'cv2'

lint run-test: commands[0] | pylint --rcfile=tests/pylintrc sksurgerycalibration tests
************* Module sksurgerycalibration.video.video_calibration_driver_stereo
sksurgerycalibration/video/video_calibration_driver_stereo.py:118:24: E1101: Module 'cv2' has no 'CALIB_USE_INTRINSIC_GUESS' member (no-member)
sksurgerycalibration/video/video_calibration_driver_stereo.py:180:43: E1101: Module 'cv2' has no 'CALIB_USE_INTRINSIC_GUESS' member (no-member)
************* Module sksurgerycalibration.video.video_calibration_data
sksurgerycalibration/video/video_calibration_data.py:210:12: E1101: Module 'cv2' has no 'imwrite' member (no-member)
sksurgerycalibration/video/video_calibration_data.py:246:20: E1101: Module 'cv2' has no 'imread' member (no-member)
sksurgerycalibration/video/video_calibration_data.py:291:15: E1101: Module 'cv2' has no 'FONT_HERSHEY_SIMPLEX' member (no-member)
sksurgerycalibration/video/video_calibration_data.py:301:16: E1101: Module 'cv2' has no 'putText' member (no-member)
sksurgerycalibration/video/video_calibration_data.py:305:55: E1101: Module 'cv2' has no 'LINE_AA' member (no-member)
sksurgerycalibration/video/video_calibration_data.py:312:12: E1101: Module 'cv2' has no 'imwrite' member (no-member)

Solutions:

pylint logs for C0301: Line too long (108/80) (line-too-long

************* Module sksurgerycalibration.video.video_calibration_metrics
sksurgerycalibration/video/video_calibration_metrics.py:153:0: C0301: Line too long (100/80) (line-too-long)
************* Module sksurgerycalibration.algorithms.triangulate
sksurgerycalibration/algorithms/triangulate.py:98:0: C0301: Line too long (108/80) (line-too-long)
sksurgerycalibration/algorithms/triangulate.py:132:0: C0301: Line too long (125/80) (line-too-long)
sksurgerycalibration/algorithms/triangulate.py:137:0: C0301: Line too long (84/80) (line-too-long)
sksurgerycalibration/algorithms/triangulate.py:160:0: C0301: Line too long (92/80) (line-too-long)
sksurgerycalibration/algorithms/triangulate.py:169:0: C0301: Line too long (100/80) (line-too-long)
sksurgerycalibration/algorithms/triangulate.py:177:0: C0301: Line too long (99/80) (line-too-long)
sksurgerycalibration/algorithms/triangulate.py:178:0: C0301: Line too long (93/80) (line-too-long)
sksurgerycalibration/algorithms/triangulate.py:179:0: C0301: Line too long (105/80) (line-too-long)
sksurgerycalibration/algorithms/triangulate.py:204:0: C0301: Line too long (112/80) (line-too-long)
sksurgerycalibration/algorithms/triangulate.py:206:0: C0301: Line too long (122/80) (line-too-long)
sksurgerycalibration/algorithms/triangulate.py:232:0: C0301: Line too long (95/80) (line-too-long)

C0103: Function name

sksurgerycalibration/algorithms/triangulate.py:10:0: C0103: Function name "internal_triangulate_point_using_svd" doesn't conform to '[a-z_][a-z0-9_]{2,30}$' pattern (invalid-name)

C0103: Argument name

************* Module sksurgerycalibration.algorithms.triangulate
sksurgerycalibration/algorithms/triangulate.py:11:33: C0103: Argument name "p2" doesn't conform to '[a-z_][a-z0-9_]{2,30}$' pattern (invalid-name)
sksurgerycalibration/algorithms/triangulate.py:12:33: C0103: Argument name "u1" doesn't conform to '[a-z_][a-z0-9_]{2,30}$' pattern (invalid-name)
sksurgerycalibration/algorithms/triangulate.py:13:33: C0103: Argument name "u2" doesn't conform to '[a-z_][a-z0-9_]{2,30}$' pattern (invalid-name)
sksurgerycalibration/algorithms/triangulate.py:14:33: C0103: Argument name "w1" doesn't conform to '[a-z_][a-z0-9_]{2,30}$' pattern (invalid-name)
sksurgerycalibration/algorithms/triangulate.py:15:33: C0103: Argument name "w2" doesn't conform to '[a-z_][a-z0-9_]{2,30}$' pattern (invalid-name)
sksurgerycalibration/algorithms/triangulate.py:37:4: C0103: Variable name "a" doesn't conform to '[a-z_][a-z0-9_]{2,30}$' pattern (invalid-name)
sksurgerycalibration/algorithms/triangulate.py:51:4: C0103: Variable name "b" doesn't conform to '[a-z_][a-z0-9_]{2,30}$' pattern (invalid-name)
sksurgerycalibration/algorithms/triangulate.py:57:4: C0103: Variable name "x" doesn't conform to '[a-z_][a-z0-9_]{2,30}$' pattern (invalid-name)
sksurgerycalibration/algorithms/triangulate.py:63:0: C0103: Function name "_iterative_triangulate_point_using_svd" doesn't conform to '[a-z_][a-z0-9_]{2,30}$' pattern (invalid-name)
sksurgerycalibration/algorithms/triangulate.py:64:43: C0103: Argument name "p2" doesn't conform to '[a-z_][a-z0-9_]{2,30}$' pattern (invalid-name)
sksurgerycalibration/algorithms/triangulate.py:65:43: C0103: Argument name "u1" doesn't conform to '[a-z_][a-z0-9_]{2,30}$' pattern (invalid-name)
sksurgerycalibration/algorithms/triangulate.py:66:43: C0103: Argument name "u2" doesn't conform to '[a-z_][a-z0-9_]{2,30}$' pattern (invalid-name)
sksurgerycalibration/algorithms/triangulate.py:84:4: C0103: Variable name "w1" doesn't conform to '[a-z_][a-z0-9_]{2,30}$' pattern (invalid-name)
sksurgerycalibration/algorithms/triangulate.py:85:4: C0103: Variable name "w2" doesn't conform to '[a-z_][a-z0-9_]{2,30}$' pattern (invalid-name)
sksurgerycalibration/algorithms/triangulate.py:86:4: C0103: Variable name "x" doesn't conform to '[a-z_][a-z0-9_]{2,30}$' pattern (invalid-name)
sksurgerycalibration/algorithms/triangulate.py:90:8: C0103: Variable name "x_" doesn't conform to '[a-z_][a-z0-9_]{2,30}$' pattern (invalid-name)
sksurgerycalibration/algorithms/triangulate.py:103:8: C0103: Variable name "w1" doesn't conform to '[a-z_][a-z0-9_]{2,30}$' pattern (invalid-name)
sksurgerycalibration/algorithms/triangulate.py:104:8: C0103: Variable name "w2" doesn't conform to '[a-z_][a-z0-9_]{2,30}$' pattern (invalid-name)
sksurgerycalibration/algorithms/triangulate.py:89:8: W0612: Unused variable 'i' (unused-variable)

W0612: Unused variable 'i' (unused-variable)

C0325: Unnecessary parens after 'if' keyword (superfluous-parens)

mxochicale commented 1 year ago

Local unit test failures:


    def test_stereo_davinci():

        left_images = []

...

        reproj_err, recon_err, params = calibrator.calibrate()
        print("Reproj:" + str(reproj_err))
        print("Recon:" + str(recon_err))
>       assert reproj_err < 1.1
E       assert 82521.00771341282 < 1.1

tests/video/test_charuco_plus_chessboard.py:55: AssertionError

...

>       assert reproj_err_1 == pytest.approx(1., rel=0.2)
E       assert 146.10223709746785 == 1.0 ± 2.0e-01
E         comparison failed
E         Obtained: 146.10223709746785
E         Expected: 1.0 ± 2.0e-01

tests/video/test_hand_eye.py:78: AssertionError

tests/video/test_precalib.py:92: AssertionError

        calib_driver.handeye_calibration()
    print(stereo_reproj_err, stereo_recon_err, tracked_reproj_err, tracked_recon_err)
  assert stereo_reproj_err < 4.5

E assert 34.74794862170866 < 4.5

tests/video/test_precalib.py:124: AssertionError

### Notes
* Steve suggested to trying from a clean repo: clone repo and run tox 
* Cloning repo and running tox passed tests ( on Fri 23 Dec 10:21:21 GMT 2022)

py37: OK (339.19=setup[16.31]+cmd[0.39,0.05,322.30,0.14] seconds) lint: OK (24.84=setup[15.34]+cmd[0.37,9.13] seconds) congratulations :) (364.18 seconds)

Running tox in a second time:

py37: OK (325.41=setup[0.02]+cmd[0.32,0.05,324.88,0.14] seconds) lint: OK (9.47=setup[0.01]+cmd[0.32,9.14] seconds) congratulations :) (335.00 seconds)

Running tox in the original cloned path

py37: OK (306.90=setup[2.35]+cmd[0.45,0.06,303.89,0.14] seconds) lint: OK (12.65=setup[2.39]+cmd[0.46,9.81] seconds) congratulations :) (319.68 seconds)

mxochicale commented 1 year ago

Reproducing TriangulatePointsUsingHartley implementation to compute rmsHartley

main.cpp

/*=============================================================================
    TriangulatePointsUsingHartley

    References:
    SKSURGERYCVCPP: scikit-surgeryopencvcpp provides opencv functions in C++
    https://github.com/SciKit-Surgery/scikit-surgeryopencvcpp/blob/master/Testing/sksTriangulateTest.cpp
    https://github.com/SciKit-Surgery/scikit-surgeryopencvcpp/blob/master/Code/Lib/sksMaths.cpp

=============================================================================*/

#include <opencv2/opencv.hpp>

namespace sks
{
    //-----------------------------------------------------------------------------
    cv::Mat_<double> InternalTriangulatePointUsingSVD(
        const cv::Matx34d& P1,
        const cv::Matx34d& P2,
        const cv::Point3d& u1,
        const cv::Point3d& u2,
        const double& w1,
        const double& w2
        )
    {
      // Build matrix A for homogeneous equation system Ax = 0
      // Assume X = (x,y,z,1), for Linear-LS method
      // Which turns it into a AX = B system, where A is 4x3, X is 3x1 and B is 4x1
      cv::Matx43d A((u1.x*P1(2,0)-P1(0,0))/w1, (u1.x*P1(2,1)-P1(0,1))/w1, (u1.x*P1(2,2)-P1(0,2))/w1,
                    (u1.y*P1(2,0)-P1(1,0))/w1, (u1.y*P1(2,1)-P1(1,1))/w1, (u1.y*P1(2,2)-P1(1,2))/w1,
                    (u2.x*P2(2,0)-P2(0,0))/w2, (u2.x*P2(2,1)-P2(0,1))/w2, (u2.x*P2(2,2)-P2(0,2))/w2,
                    (u2.y*P2(2,0)-P2(1,0))/w2, (u2.y*P2(2,1)-P2(1,1))/w2, (u2.y*P2(2,2)-P2(1,2))/w2
                   );

      cv::Matx41d B(-(u1.x*P1(2,3) -P1(0,3))/w1,
                    -(u1.y*P1(2,3) -P1(1,3))/w1,
                    -(u2.x*P2(2,3) -P2(0,3))/w2,
                    -(u2.y*P2(2,3) -P2(1,3))/w2
                   );

      cv::Mat_<double> X;
      cv::solve(A,B,X,cv::DECOMP_SVD);

      return X;
    }

    //-----------------------------------------------------------------------------
    cv::Point3d InternalIterativeTriangulatePointUsingSVD(
        const cv::Matx34d& P1,
        const cv::Matx34d& P2,
        const cv::Point3d& u1,
        const cv::Point3d& u2
        )
    {
      double epsilon = 0.00000000001;
      double w1 = 1, w2 = 1;
      cv::Mat_<double> X(4,1);

      for (int i=0; i<10; i++) // Hartley suggests 10 iterations at most
      {
        cv::Mat_<double> X_ = InternalTriangulatePointUsingSVD(P1,P2,u1,u2,w1,w2);
        X(0) = X_(0);
        X(1) = X_(1);
        X(2) = X_(2);
        X(3) = 1.0;

        double p2x1 = cv::Mat_<double>(cv::Mat_<double>(P1).row(2)*X)(0);
        double p2x2 = cv::Mat_<double>(cv::Mat_<double>(P2).row(2)*X)(0);

        if(fabs(w1 - p2x1) <= epsilon && fabs(w2 - p2x2) <= epsilon)
          break;

        w1 = p2x1;
        w2 = p2x2;
      }

      cv::Point3d result;
      result.x = X(0);
      result.y = X(1);
      result.z = X(2);

      return result;
    }

    //-----------------------------------------------------------------------------
    cv::Mat TriangulatePointsUsingHartley(
      const cv::Mat& inputUndistortedPoints,
      const cv::Mat& leftCameraIntrinsicParams,
      const cv::Mat& rightCameraIntrinsicParams,
      const cv::Mat& leftToRightRotationMatrix,
      const cv::Mat& leftToRightTranslationVector
      )
    {

      int numberOfPoints = inputUndistortedPoints.rows;

      cv::Mat outputPoints = cv::Mat(numberOfPoints, 3, CV_64FC1);

      cv::Mat K1    = cv::Mat(3, 3, CV_64FC1);
      cv::Mat K2    = cv::Mat(3, 3, CV_64FC1);
      cv::Mat K1Inv = cv::Mat(3, 3, CV_64FC1);
      cv::Mat K2Inv = cv::Mat(3, 3, CV_64FC1);
      cv::Mat R1    = cv::Mat::eye(3, 3, CV_64FC1);
      cv::Mat R2    = leftToRightRotationMatrix;
      cv::Mat E1    = cv::Mat::eye(4, 4, CV_64FC1);
      cv::Mat E1Inv = cv::Mat::eye(4, 4, CV_64FC1);
      cv::Mat E2    = cv::Mat::eye(4, 4, CV_64FC1);
      cv::Mat L2R   = cv::Mat(4, 4, CV_64FC1);
      cv::Matx34d P1d, P2d;

      // Construct:
      // E1 = Object to Left Camera = Left Camera Extrinsics.
      // E2 = Object to Right Camera = Right Camera Extrinsics.
      // K1 = Copy of Left Camera intrinsics.
      // K2 = Copy of Right Camera intrinsics.
      // Copy data into cv::Mat data types.
      // Camera calibration routines are 32 bit, as some drawing functions require 32 bit data.
      // These triangulation routines need 64 bit data.
      for (int r = 0; r < 3; r++)
      {
        for (int c = 0; c < 3; c++)
        {
          K1.at<double>(r, c) = leftCameraIntrinsicParams.at<double>(r, c);
          K2.at<double>(r, c) = rightCameraIntrinsicParams.at<double>(r, c);
          E2.at<double>(r, c) = R2.at<double>(r, c);
        }
        E2.at<double>(r, 3) = leftToRightTranslationVector.at<double>(r, 0);
      }

      // We invert the intrinsic params, so we can convert from pixels to normalised image coordinates.
      K1Inv = K1.inv();
      K2Inv = K2.inv();

      // We want output coordinates relative to left camera.
      E1Inv = E1.inv();
      L2R = E2 * E1Inv;

      // Reading Prince 2012 Computer Vision, the projection matrix, is just the extrinsic parameters,
      // as our coordinates will be in a normalised camera space. P1 should be identity, so that
      // reconstructed coordinates are in Left Camera Space, to P2 should reflect a right to left transform.
      P1d(0,0) = 1; P1d(0,1) = 0; P1d(0,2) = 0; P1d(0,3) = 0;
      P1d(1,0) = 0; P1d(1,1) = 1; P1d(1,2) = 0; P1d(1,3) = 0;
      P1d(2,0) = 0; P1d(2,1) = 0; P1d(2,2) = 1; P1d(2,3) = 0;

      for (int i = 0; i < 3; i++)
      {
        for (int j = 0; j < 4; j++)
        {
          P2d(i,j) = L2R.at<double>(i,j);
        }
      }

      #pragma omp parallel
      {
        cv::Mat u1    = cv::Mat(3, 1, CV_64FC1);
        cv::Mat u2    = cv::Mat(3, 1, CV_64FC1);
        cv::Mat u1t   = cv::Mat(3, 1, CV_64FC1);
        cv::Mat u2t   = cv::Mat(3, 1, CV_64FC1);

        cv::Point3d u1p, u2p;            // Normalised image coordinates. (i.e. relative to a principal point of zero, and in millimetres not pixels).
        cv::Point3d reconstructedPoint;  // the output 3D point, in reference frame of left camera.

        #pragma omp for
        for (int i = 0; i < numberOfPoints; i++)
        {
          u1.at<double>(0,0) = inputUndistortedPoints.at<double>(i, 0);
          u1.at<double>(1,0) = inputUndistortedPoints.at<double>(i, 1);
          u1.at<double>(2,0) = 1;

          u2.at<double>(0,0) = inputUndistortedPoints.at<double>(i, 2);
          u2.at<double>(1,0) = inputUndistortedPoints.at<double>(i, 3);
          u2.at<double>(2,0) = 1;

          // Converting to normalised image points
          u1t = K1Inv * u1;
          u2t = K2Inv * u2;

          u1p.x = u1t.at<double>(0,0);
          u1p.y = u1t.at<double>(1,0);
          u1p.z = u1t.at<double>(2,0);

          u2p.x = u2t.at<double>(0,0);
          u2p.y = u2t.at<double>(1,0);
          u2p.z = u2t.at<double>(2,0);

          reconstructedPoint = InternalIterativeTriangulatePointUsingSVD(P1d, P2d, u1p, u2p);

          outputPoints.at<double>(i, 0) = reconstructedPoint.x;
          outputPoints.at<double>(i, 1) = reconstructedPoint.y;
          outputPoints.at<double>(i, 2) = reconstructedPoint.z;
        } // end for
      } // end parallel block
      return outputPoints;
    }

    //-----------------------------------------------------------------------------
    double ComputeRMSBetweenCorrespondingPoints(const cv::Mat& a, const cv::Mat& b)
    {
      double rms = 0;
      double diff = 0;
      double squaredDiff = 0;

      if (a.rows != b.rows)
      {
        std::cout << "a has " << a.rows << " rows, but b has " << b.rows;
      }

      if (a.cols != 3)
      {
        std::cout << "a does not have 3 columns.";
      }

      if (b.cols != 3)
      {
        std::cout << "b does not have 3 columns.";
      }

      for (unsigned int r = 0; r < a.rows; r++)
      {
        for (unsigned int c = 0; c < a.cols; c++)
        {
          diff = (b.at<double>(r, c) - a.at<double>(r, c));
          squaredDiff = diff * diff;
          rms += squaredDiff;
        }
      }
      rms /= static_cast<double>(a.rows);
      rms = std::sqrt(rms);
      return rms;
    }

}

int main(int, char**)
{

    std::cout << "Hello, OpenCV\n";

    cv::Mat leftIntrinsic = cv::Mat::eye(3, 3, CV_64FC1);
    leftIntrinsic.at<double>(0, 0) = 2012.186314;
    leftIntrinsic.at<double>(1, 1) = 2017.966019;
    leftIntrinsic.at<double>(0, 2) = 944.7173708;
    leftIntrinsic.at<double>(1, 2) = 617.1093984;

    cv::Mat rightIntrinsic = cv::Mat::eye(3, 3, CV_64FC1);
    rightIntrinsic.at<double>(0, 0) = 2037.233928;
    rightIntrinsic.at<double>(1, 1) = 2052.018948;
    rightIntrinsic.at<double>(0, 2) = 1051.112809;
    rightIntrinsic.at<double>(1, 2) = 548.0675962;

    cv::Mat leftRotation = cv::Mat::eye(3, 3, CV_64FC1);
    leftRotation.at<double>(0, 0) = 0.966285949;
    leftRotation.at<double>(0, 1) = -0.1053020017;
    leftRotation.at<double>(0, 2) = 0.2349530874;
    leftRotation.at<double>(1, 0) = -0.005105986897;
    leftRotation.at<double>(1, 1) = 0.9045241988;
    leftRotation.at<double>(1, 2) = 0.4263917244;
    leftRotation.at<double>(2, 0) = -0.2574206552;
    leftRotation.at<double>(2, 1) = -0.4132159994;
    leftRotation.at<double>(2, 2) = 0.8734913532;

    cv::Mat leftTranslation = cv::Mat::eye(3, 1, CV_64FC1);
    leftTranslation.at<double>(0, 0) = 9.847672184;
    leftTranslation.at<double>(1, 0) = -22.45992103;
    leftTranslation.at<double>(2, 0) = 127.7836183;

    cv::Mat rightRotation = cv::Mat::eye(3, 3, CV_64FC1);
    rightRotation.at<double>(0, 0) = 0.958512625;
    rightRotation.at<double>(0, 1) = -0.1175427792;
    rightRotation.at<double>(0, 2) = 0.2596868167;
    rightRotation.at<double>(1, 0) = -0.0115032253;
    rightRotation.at<double>(1, 1) = 0.8943292319;
    rightRotation.at<double>(1, 2) = 0.4472615574;
    rightRotation.at<double>(2, 0) = -0.2848178778;
    rightRotation.at<double>(2, 1) = -0.4316930854;
    rightRotation.at<double>(2, 2) = 0.8558737386;

    cv::Mat rightTranslation = cv::Mat::eye(3, 1, CV_64FC1);
    rightTranslation.at<double>(0, 0) = 8.461514886;
    rightTranslation.at<double>(1, 0) = -19.3242747;
    rightTranslation.at<double>(2, 0) = 129.0937601;

    cv::Mat pointsIn2D = cv::Mat::zeros(4, 4, CV_64FC1);
    pointsIn2D.at<double>(0, 0) = 1100.16;
    pointsIn2D.at<double>(0, 1) = 262.974;
    pointsIn2D.at<double>(0, 2) = 1184.84;
    pointsIn2D.at<double>(0, 3) = 241.915;
    pointsIn2D.at<double>(1, 0) = 1757.74;
    pointsIn2D.at<double>(1, 1) = 228.971;
    pointsIn2D.at<double>(1, 2) = 1843.52;
    pointsIn2D.at<double>(1, 3) = 204.083;
    pointsIn2D.at<double>(2, 0) = 1065.44;
    pointsIn2D.at<double>(2, 1) = 651.593;
    pointsIn2D.at<double>(2, 2) = 1142.75;
    pointsIn2D.at<double>(2, 3) = 632.817;
    pointsIn2D.at<double>(3, 0) = 1788.22;
    pointsIn2D.at<double>(3, 1) = 650.41;
    pointsIn2D.at<double>(3, 2) = 1867.78;
    pointsIn2D.at<double>(3, 3) = 632.59;

    cv::Mat leftToRightRotation = cv::Mat::eye(3, 3, CV_64FC1);
    leftToRightRotation.at<double>(0, 0) = 0.999678;
    leftToRightRotation.at<double>(0, 1) = 0.000151;
    leftToRightRotation.at<double>(0, 2) = 0.025398;
    leftToRightRotation.at<double>(1, 0) = -0.000720;
    leftToRightRotation.at<double>(1, 1) = 0.999749;
    leftToRightRotation.at<double>(1, 2) = 0.022394;
    leftToRightRotation.at<double>(2, 0) = -0.025388;
    leftToRightRotation.at<double>(2, 1) = -0.022405;
    leftToRightRotation.at<double>(2, 2) = 0.999426;

    cv::Mat leftToRightTranslation = cv::Mat::eye(3, 1, CV_64FC1);
    leftToRightTranslation.at<double>(0, 0) = -4.631472;
    leftToRightTranslation.at<double>(1, 0) = 0.268695;
    leftToRightTranslation.at<double>(2, 0) = 1.300256;

    cv::Mat modelPoints = cv::Mat::eye(4, 3, CV_64FC1);
    modelPoints.at<double>(0, 0) = 0;
    modelPoints.at<double>(0, 1) = 0;
    modelPoints.at<double>(0, 2) = 0;
    modelPoints.at<double>(1, 0) = 39;
    modelPoints.at<double>(1, 1) = 0;
    modelPoints.at<double>(1, 2) = 0;
    modelPoints.at<double>(2, 0) = 0;
    modelPoints.at<double>(2, 1) = 27;
    modelPoints.at<double>(2, 2) = 0;
    modelPoints.at<double>(3, 0) = 39;
    modelPoints.at<double>(3, 1) = 27;
    modelPoints.at<double>(3, 2) = 0;

    cv::Mat modelPointsTransposed;
    cv::transpose(modelPoints, modelPointsTransposed);
    std::cout << "modelPointsTransposed: \n" << modelPointsTransposed << std::endl;

    cv::Mat rotatedModelPoints = cv::Mat(modelPointsTransposed.rows, modelPointsTransposed.cols, CV_64FC1);
    cv::gemm(leftRotation, modelPointsTransposed, 1, cv::Mat(), 0, rotatedModelPoints);
    std::cout << "rotatedModelPoints: \n" << rotatedModelPoints << std::endl;

    cv::Mat modelPointsRotatedAndTransposed;
    cv::transpose(rotatedModelPoints, modelPointsRotatedAndTransposed);
    std::cout << "modelPointsRotatedAndTransposed: \n" << modelPointsRotatedAndTransposed << std::endl;

    cv::Mat transformedModelPoints = cv::Mat(modelPointsRotatedAndTransposed.rows, modelPointsRotatedAndTransposed.cols, CV_64FC1);
    for (int r = 0; r < modelPointsRotatedAndTransposed.rows; r++)
        {
            for (int c = 0; c < modelPointsRotatedAndTransposed.cols; c++)
                {
                transformedModelPoints.at<double>(r, c) = modelPointsRotatedAndTransposed.at<double>(r, c) + leftTranslation.at<double>(c, 0);
                }
        }

    std::cout << "transformedModelPoints: \n" << transformedModelPoints << std::endl;

    std::cout << "TriangulatePointsUsingHartley> pointsIn2D: \n" << pointsIn2D << std::endl;
    std::cout << "TriangulatePointsUsingHartley> leftIntrinsic: \n" << leftIntrinsic << std::endl;
    std::cout << "TriangulatePointsUsingHartley> rightIntrinsic: \n" << rightIntrinsic << std::endl;
    std::cout << "TriangulatePointsUsingHartley> leftToRightRotation: \n" << leftToRightRotation << std::endl;
    std::cout << "TriangulatePointsUsingHartley> leftToRightTranslation: \n" << leftToRightTranslation << std::endl;

    cv::Mat pointsFromHartley = sks::TriangulatePointsUsingHartley(pointsIn2D,
                                                                 leftIntrinsic,
                                                                 rightIntrinsic,
                                                                 leftToRightRotation,
                                                                 leftToRightTranslation
                                                                );
    std::cout << "pointsFromHartley: \n" << pointsFromHartley << std::endl;

    double rmsHartley = sks::ComputeRMSBetweenCorrespondingPoints(transformedModelPoints, pointsFromHartley);
    std::cout << "rmsHartley: \n" << rmsHartley << std::endl;

    return 0;
}

logs

TriangulatePointsUsingHartley> pointsIn2D: 
[1100.16, 262.974, 1184.84, 241.915;
 1757.74, 228.971, 1843.52, 204.083;
 1065.44, 651.593, 1142.75, 632.817;
 1788.22, 650.41, 1867.78, 632.59]
TriangulatePointsUsingHartley> leftIntrinsic: 
[2012.186314, 0, 944.7173708;
 0, 2017.966019, 617.1093984;
 0, 0, 1]
TriangulatePointsUsingHartley> rightIntrinsic: 
[2037.233928, 0, 1051.112809;
 0, 2052.018948, 548.0675962;
 0, 0, 1]
TriangulatePointsUsingHartley> leftToRightRotation: 
[0.999678, 0.000151, 0.025398;
 -0.00072, 0.999749, 0.022394;
 -0.025388, -0.022405, 0.999426]
TriangulatePointsUsingHartley> leftToRightTranslation: 
[-4.631472;
 0.268695;
 1.300256]
pointsFromHartley: 
[9.882561793478274, -22.44358672047434, 127.9216596991245;
 48.46473971483012, -23.09607651651325, 119.9524462287327;
 6.946803109459471, 1.973326991586282, 115.7921729428552;
 44.77458535398358, 1.768363607856355, 106.8097293986459]
rmsHartley: 
1.29547
mxochicale commented 1 year ago
mxochicale commented 1 year ago

Steve and I discussed next steps: to remove import sksurgeryopencvpython as cvpy from video_calibration_metrics.py and remove scikit-surgeryopencvcpp in requirements.text and setup.py

mxochicale commented 1 year ago 
************* Module sksurgerycalibration.algorithms.triangulate
sksurgerycalibration/algorithms/triangulate.py:113:0: R0914: Too many local variables (31/30) (too-many-locals)

:tada: SORTED OUT > https://github.com/SciKit-Surgery/scikit-surgerycalibration/commit/4cc816a664cc87fb2c78e8c7d22fbee65b510031

mxochicale commented 1 year ago

Notes: cv2.solve appears to be faster than np.linalg.lstsq:

LOGS

100124 nsecs 34191 nsecs 19551 nsecs 16927 nsecs 15039 nsecs 14558 nsecs 14539 nsecs 15572 nsecs 14425 nsecs 14266 nsecs 14468 nsecs 14576 nsecs 14288 nsecs

rms_hartley: 1.2954729076605125

* `cv2.solve`

CODE

start = time.time_ns()
x_array = cv2.solve(a_array, b_array, flags=cv2.DECOMP_SVD)
# x_array, _, _, _ = np.linalg.lstsq(a_array, b_array, rcond=-1) #Alternatively
print(f'{ time.time_ns() - start } nsecs')

return x_array[1] #for cv2.solve #x_array#for np.linalg.lstsq

LOGS

62866 nsecs 4518 nsecs 2809 nsecs 2494 nsecs 2471 nsecs 2363 nsecs 2323 nsecs 2403 nsecs 2240 nsecs 2299 nsecs 2384 nsecs 2335 nsecs 2340 nsecs

rms_hartley: 1.2954729076604021

mxochicale commented 1 year ago

Summary

Few final bits to resolve:



* [x] check usage of `cv2.triangulatePoints()` www.programcreek.com/python/example/110665/cv2.triangulatePoints ; https://python.hotexamples.com/examples/cv2/-/triangulatePoints/python-triangulatepoints-function-examples.html 
* [x] test results `triangulate_points_opencv` and `test_triangulate_points_opencv`