I am writing a script to take pictures with a fixed camera and calculate the distance between the center of each object in the picture and an arbitrary origin (around the center of the camera). Each picture that the camera takes goes through an "undistortion" script that applies the calibration values (obtained with a modified calibration script and saved to a yaml file). The distance values are off from what I can measure with a caliper.
Code snippet down here. The original code is longer, I only left the parts that capture and handle the image; the VideoCapture class was taken from a post online.
import time
import cv2
import numpy as np
import yaml
import imutils
from imutils import contours, perspective
from scipy.spatial import distance
import threading
import queue
import datetime
class VideoCapture:
def __init__(self, name):
self.cap = cv2.VideoCapture(name, cv2.CAP_DSHOW)
self.q = queue.Queue()
t = threading.Thread(target=self._reader)
t.daemon = True
t.start()
def _reader(self):
while True:
ret, frame = self.cap.read()
if not ret:
break
if not self.q.empty():
try:
self.q.get_nowait()
except queue.Empty:
pass
self.q.put(frame)
def read(self):
return self.q.get()
def readAndUndistort(cap, cal_par): #cal_par = [mtx,dist,w,h]
raw_img = cap.read()
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(cal_par[0], cal_par[1], (cal_par[2],cal_par[3]), 1, (cal_par[2],cal_par[3]))
mapx, mapy = cv2.initUndistortRectifyMap(cal_par[0], cal_par[1], None, newcameramtx, (cal_par[2],cal_par[3]), 5)
return cv2.remap(raw_img, mapx, mapy, cv2.INTER_LINEAR, cv2.BORDER_CONSTANT)
def midpoint(ptA, ptB):
return (float((ptA[0] + ptB[0])) * 0.5, float((ptA[1] + ptB[1])) * 0.5)
def calcPixelsPerMM(img, height_real, width_real = None): #Currently not in use, I applied a fixed value
width_real = height_real if width_real is None else width_real
search_area = img[int(roi[1]):int(roi[1]+roi[3]),
int(roi[0]):int(roi[0]+roi[2])]
gray = cv2.cvtColor(search_area, cv2.COLOR_BGR2GRAY)
gray = cv2.GaussianBlur(gray, (7, 7), 0)
edged = cv2.Canny(gray, 0, 80)
cnts, _ = cv2.findContours(edged.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
c = cnts[0]
box = cv2.minAreaRect(c)
box = cv2.boxPoints(box)
box = perspective.order_points(box)
(tl, tr, br, bl) = box
(tltrX, tltrY) = midpoint(tl, tr)
(blbrX, blbrY) = midpoint(bl, br)
(tlblX, tlblY) = midpoint(tl, bl)
(trbrX, trbrY) = midpoint(tr, br)
height_px = distance.euclidean((tltrX, tltrY), (blbrX, blbrY))
width_px = distance.euclidean((tlblX, tlblY), (trbrX, trbrY))
print(f'Height_px: {height_px}, Width_px: {width_px}')
return height_px/height_real, width_px/width_real
def findOrigin(img):
global ORIG
search_area = img[int(roi[1]):int(roi[1]+roi[3]),
int(roi[0]):int(roi[0]+roi[2])]
gray = cv2.cvtColor(search_area, cv2.COLOR_BGR2GRAY)
gray = cv2.GaussianBlur(gray, (7, 7), 0)
edged = cv2.Canny(gray, 0, 80)
cnts, _ = cv2.findContours(edged.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
c = cnts[0]
box = cv2.minAreaRect(c)
box = cv2.boxPoints(box)
box = perspective.order_points(box)
#(tl, tr, br, bl) = box
#ORIG = tl
ORIG = getCentroid(c)
cv2.line(search_area, np.array(ORIG, dtype="int"), (int(ORIG[0])+100, int(ORIG[1])), (0, 0, 255), 1)
cv2.line(search_area, np.array(ORIG, dtype="int"), (int(ORIG[0]), int(ORIG[1])+100), (0, 255,0), 1)
#print(ORIG)
return search_area
def getCoords():
undist = readAndUndistort(cam, [mtx, dist, w, h])
search_area = undist[int(roi[1]):int(roi[1]+roi[3]),
int(roi[0]):int(roi[0]+roi[2])]
gray = cv2.cvtColor(search_area, cv2.COLOR_BGR2GRAY)
gray = cv2.GaussianBlur(gray, (7, 7), 0)
edged = cv2.Canny(gray, 0, 80)
edged = cv2.dilate(edged, None, iterations=1)
cnts = cv2.findContours(edged.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
cnts = imutils.grab_contours(cnts)
(cnts, _) = contours.sort_contours(cnts)
orig = search_area.copy()
cv2.line(orig, np.array(ORIG, dtype="int"), (int(ORIG[0])+100, int(ORIG[1])), (0, 0, 255), 1)
cv2.line(orig, np.array(ORIG, dtype="int"), (int(ORIG[0]), int(ORIG[1])+100), (0, 255,0), 1)
if len(cnts) > 1:
for c in cnts:
if cv2.contourArea(c) < 100 and cv2.contourArea(c) > 150000:
continue
box = cv2.minAreaRect(c)
box = cv2.boxPoints(box)
box = np.array(box, dtype="int")
box = perspective.order_points(box)
(tl, tr, br, bl) = box
M = cv2.moments(c)
cx = cy = 0
if M['m00'] != 0:
cx = int(M['m10']/M['m00'])
cy = int(M['m01']/M['m00'])
cv2.circle(orig, (cx, cy), 3, (0, 0, 255), -1)
posAdjX = -1 if cx < ORIG[0] else 1
posAdjY = -1 if cy < ORIG[1] else 1
#print(posAdjX, posAdjY)
(coyX, coyY) = midpoint(ORIG, (ORIG[0],cy))
(coxX, coxY) = midpoint(ORIG, (cx,ORIG[1]))
tolerance = 10
if ORIG[0] - tolerance <= tl[0] <= ORIG[0] + tolerance and ORIG[1] - tolerance <= tl[1] <= ORIG[1] + tolerance:
continue
cv2.drawContours(orig, [box.astype("int")], -1, (0, 255, 0), 2)
cv2.line(orig, np.array(ORIG, dtype="int"), (int(ORIG[0]),cy),(0, 255,0), 2)
cv2.line(orig, (int(ORIG[0]),cy), (cx,cy),(0, 255, 0), 2)
cv2.line(orig, np.array(ORIG, dtype="int"), (cx, int(ORIG[1])),(0, 0, 255), 2)
cv2.line(orig, (cx, int(ORIG[1])), (cx,cy),(0, 0, 255), 2)
dCOx = distance.euclidean(ORIG, (cx, ORIG[1]))
dCOy = distance.euclidean(ORIG, (ORIG[0], cy))
dimCOx = float(dCOx)/float(pixelsPerMMx) * posAdjX
dimCOy = float(dCOy)/float(pixelsPerMMy) * posAdjY
cv2.putText(orig, f"({int(dimCOx)}, {int(dimCOy)})",(cx, cy), cv2.FONT_HERSHEY_SIMPLEX,0.65, (255, 255, 255), 2)
cv2.imshow("Result", orig)
cv2.waitKey(1000)
cv2.destroyWindow("Result")
return dimCOx, dimCOy
else:
return None
cam = VideoCapture(0)
[mtx, dist, w, h] = readCalib("camera_calib.yaml")
roi_img = readAndUndistort(cam, [mtx,dist,w,h])
roi = ROISelection(roi_img, "Seleziona area di ricerca")
pixelsPerMMy = pixelsPerMMx = 2.075 #fixed value since it doesn't change much between runs
def_img = findOrigin(roi_img)
x,y = getCoords()
This conversion algorithm is very simple, which I suspect is also why it is so inaccurate. There must be a better way to calculate distances (and therefore coordinates) from the origin.
EDIT: here are the camera matrix and distortion coefficients I get from cv2.calibrateCamera and I then use to undistort the pictures. They were taken from the yaml file I save them to (converted from numpy arrays to lists). Width is 640 and height 480.
camera_matrix:
- - 733.0876152107618
- 0.0
- 324.4980857211455
- - 0.0
- 732.2745256469318
- 262.8575866011689
- - 0.0
- 0.0
- 1.0
dist_coeff:
- - -0.4026036749641984
- 0.2837514169706547
- 0.0007766050126771588
- 0.00011414032785012568
- -0.011067012205441455
This is a sample image with the origin in the middle and the objects I'd like to get the coordinates for.
