Hi,

Thanks to all of your tips, we’ve been able to calibrate the camera using 50 saved images. However, when we run our solvePnP algorithm, we’re experiencing a peculiar problem: the outputs vary significantly for minor changes in the inputs…

Here is the code used for solvePnP along with the printed results for two similar frames (apologize for it not being that organized):

# Compute real-world distances/angles based on the rectangles

def compute_output_values(leftRect, rightRect, centerY):

```
# Calculate the left verticies
leftVerticies = cv2.boxPoints(leftRect);
# Calculate the right verticies
rightVerticies = cv2.boxPoints(rightRect);
# Combine the left and right into one verticies array
image_points = np.concatenate((leftVerticies, rightVerticies));
image_points[:,0] -= (visionConstants.width/2);
image_points[:,1] -= (centerY);
image_points[:,1] *= -1;
print (image_points);
print (visionConstants.model_points);
# Compute robot orientation
(ret, rvec, tvec) = cv2.solvePnP (visionConstants.model_points, image_points, visionConstants.mat, visionConstants.dist_coeffs);
# Compute the necessary output distance and angles
x = tvec[0][0]
y = tvec[1][0]
z = tvec[2][0]
# distance in the horizontal plane between camera and target
distance = math.sqrt(x**2 + z**2)
# horizontal angle between camera center line and target
angle1 = math.atan2(x, z)
rot, _ = cv2.Rodrigues(rvec)
rot_inv = rot.transpose()
pzero_world = np.matmul(rot_inv, -tvec)
angle2 = math.atan2(pzero_world[0][0], pzero_world[2][0])
print ("Distance: %f, Angle1: %f, Angle2: %f, X: %f, Y: %f, Z: %f, CenterY: %f" % (distance, angle1, angle2, x, y, z, centerY));
return distance, angle1, angle2
```

*## Camera constants*

dist_coeffs = np.matrix([-5.10E-02, 9.89E-04, 3.08E-03, -4.16E-03, -6.69E-06]);

mat = np.matrix([[6.88E+01, 0, 2.44E+02], [0, 9.78E+01, 1.85E+02], [0, 0, 1]]);

And printed results:

```
[[ -31.70584106 -35.67059326]
[ -59.70584106 -28.67059326]
[ -44.23526001 33.21173096]
[ -16.23526001 26.21173096]
[ 82.90585327 -33.42352295]
[ 68.90585327 29.57649231]
[ 97.28231812 35.88238525]
[ 111.28231812 -27.11761475]]
[[-5.37709 -3.199812 0. ]
[-6.69996 -2.699812 0. ]
[-5.32288 2.625 0. ]
[-4. 2.125 0. ]
[ 5.37709 -3.199812 0. ]
[ 4. 2.125 0. ]
[ 5.32288 2.625 0. ]
[ 6.69996 -2.699812 0. ]]
Distance: 1.469959, Angle1: -1.423479, Angle2: 1.618986, X: -1.454037, Y: 0.494368, Z: 0.215768, CenterY: 280.152954
[[ -31.70584106 -35.75 ]
[ -59.70584106 -28.75 ]
[ -44.23526001 33.13232422]
[ -16.23526001 26.13232422]
[ 84.11767578 -33.39706421]
[ 68.52941895 28.95588684]
[ 96.76470947 36.01473999]
[ 112.35296631 -26.3381958 ]]
[[-5.37709 -3.199812 0. ]
[-6.69996 -2.699812 0. ]
[-5.32288 2.625 0. ]
[-4. 2.125 0. ]
[ 5.37709 -3.199812 0. ]
[ 4. 2.125 0. ]
[ 5.32288 2.625 0. ]
[ 6.69996 -2.699812 0. ]]
Distance: 4.526347, Angle1: -0.658742, Angle2: 1.571939, X: -2.770679, Y: 0.855593, Z: 3.579267, CenterY: 280.073547
```

Any ideas? Thanks in advance

Edit: I noticed a non-FRC user of solvePnP experience a similar problem here but I’m not sure how they solved it (if they did).

Edit: We realized that our problem was that the rectangles were not exact rectangles but rather had a strong blur to them. We isolated the problem to be the fact that we tried to use IR lightning and it is possible that the camera can not produce the same resolution for IR. We decided to switch back to the traditional lightning, and, with a new camera, initial testing is showing that this problem is significantly decreased. I’ll update on how it goes tomorrow.