def drawAxisOnLane(self, perspectiveImage):
be_corner = np.float32(
[[self.curDstRoadCorners[0][0],
self.curDstRoadCorners[0][1], 0],
[self.curDstRoadCorners[0][0],
self.curDstRoadCorners[0][1], 0],
[self.curDstRoadCorners[0][0],
self.curDstRoadCorners[0][1], 0]]).reshape(-1, 3)
be_axis = np.float32(
[[self.curDstRoadCorners[0][0] + 64,
self.curDstRoadCorners[0][1], 0],
[self.curDstRoadCorners[0][0],
self.curDstRoadCorners[0][1] + 128, 0],
[self.curDstRoadCorners[0][0],
self.curDstRoadCorners[0][1], -64]]).reshape(-1, 3)
corner, jac = cv2.projectPoints(
be_corner, self.rvecs, self.tvecs, self.mtx, self.dist)
axis, jac = cv2.projectPoints(
be_axis, self.rvecs, self.tvecs, self.mtx, self.dist)
corner = tuple(corner[0].ravel())
cv2.line(perspectiveImage, corner, tuple(
axis[0].ravel()), (255, 0, 0), 5)
cv2.line(perspectiveImage, corner, tuple(
axis[1].ravel()), (0, 255, 0), 5)
cv2.line(perspectiveImage, corner, tuple(
axis[2].ravel()), (0, 0, 255), 5)
def draw_things(rvec, tvec, cam_matrix, dist_coefs):
COLOR_FRAME = [0, 255, 0]
COLOR_MARKER = [0, 255, 255]
side_w = 0.5
dx, dy, dz = 0 - (side_w /2.0), 0 - (side_w /2.0), 0 - (side_w /2.0)
shift_v = lambda v: [v[0] + dx, v[1] + dy, v[2] + dz]
sides = []
base = [[0, 0], [side_w, 0], [side_w, side_w], [0.0, side_w]]
for i in xrange(3):
for c in [0, side_w]:
sides.append(map(shift_v, [ v[:i] + [c] + v[i:] for v in base]))
for i in xrange(len(sides)):
proj, _ = cv2.projectPoints(np.float32(sides[i]), rvec, tvec, cam_matrix, dist_coefs)
proj = np.int32(map(lambda x: x[0], proj))
cv2.polylines(frame, [proj], True, COLOR_FRAME)
# draw marker on front side of cube
front_side_marker = map(shift_v, [[0.0, side_w / 2.0, side_w], [side_w, side_w / 2.0, side_w],
[side_w / 2.0, 0.0, side_w], [side_w / 2.0, side_w, side_w]])
proj, _ = cv2.projectPoints(np.float32(front_side_marker), rvec, tvec, cam_matrix, dist_coefs)
proj = np.int32(map(lambda x: x[0], proj))
cv2.polylines(frame, [proj], True, COLOR_MARKER)
def imagePubCB(event):
camPos = pose[0:3]
camOrient = pose[3:7]
targetPos = pose[7:10]
targetOrient = pose[10:]
# Transform
br.sendTransform(targetPos,targetOrient,rospy.Time.now(),"ugv0","world")
# Inverse camera pose
coiInv = qInv(camOrient)
cpiInv = rotateVec(-camPos,coiInv)
(rvec,jac) = cv2.Rodrigues(q2m(coiInv)[0:3,0:3])
# Image Projections
(imagePoint,jac) = cv2.projectPoints(np.array([[targetPos]]),rvec,cpiInv,camMat,distCoeffs)
imagePoint = np.squeeze(imagePoint)
(bearing,jac) = cv2.projectPoints(np.array([[targetPos]]),rvec,cpiInv,np.eye(3),np.zeros(5))
bearing = np.squeeze(bearing)
# Publish image point
centMsg = Center()
centMsg.header.stamp = rospy.Time.now()
centMsg.header.frame_id = str(markerID)
centMsg.x = imagePoint[0]
centMsg.y = imagePoint[1]
centMsg.x1 = bearing[0]
centMsg.x2 = bearing[1]
centerPub.publish(centMsg)
def render(self, CSH, hammer, other, image, axisList):
# load calibration data
with np.load('../params/webcam_calibration_ouput.npz') as X:
mtx, dist, _, _ = [X[i] for i in ('mtx', 'dist', 'rvecs', 'tvecs')]
# set up criteria, object points and axis
criteria = (cv2.TERM_CRITERIA_EPS +
cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
objp = np.zeros((6 * 7, 3), np.float32)
objp[:, :2] = np.mgrid[0:7, 0:6].T.reshape(-1, 2)
# find grid corners in image
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, (7, 9), None)
if ret == True:
cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
rvecs, tvecs, _ = cv2.solvePnPRansac(objp, corners, mtx, dist)
if CSH == True:
# project 3D points to image plane
for axis in axisList:
imgpts, _ = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)
self._draw_cube(image, imgpts)
return image
elif hammer == True:
imgptsOther, _ = cv2.projectPoints(
other, rvecs, tvecs, mtx, dist)
self._draw_cube(image, imgptsOther)
return image
def test_projectPoints(self):
objpt = np.float64([[1,2,3]])
imgpt0, jac0 = cv2.projectPoints(objpt, np.zeros(3), np.zeros(3), np.eye(3), np.float64([]))
imgpt1, jac1 = cv2.projectPoints(objpt, np.zeros(3), np.zeros(3), np.eye(3), None)
self.assertEqual(imgpt0.shape, (objpt.shape[0], 1, 2))
self.assertEqual(imgpt1.shape, imgpt0.shape)
self.assertEqual(jac0.shape, jac1.shape)
self.assertEqual(jac0.shape[0], 2*objpt.shape[0])
def DrawGrid(self, img, rvec, tvec):
if self.camerainfo is not None:
hLinspace = (N.array(range(self.nRows)) - (self.nCols-1)/2) * self.checker_size
hZeros = N.zeros(hLinspace.shape)
hOnes = N.ones(hLinspace.shape) * self.checker_size * (self.nCols - 1)/2
vLinspace = (N.array(range(self.nCols)) - (self.nRows-1)/2) * self.checker_size
vZeros = N.zeros(vLinspace.shape)
vOnes = N.ones(vLinspace.shape) * self.checker_size * (self.nRows - 1)/2
hStart = N.array([-hOnes, hLinspace, hZeros]).astype('float32')
hEnd = N.array([ hOnes, hLinspace, hZeros]).astype('float32')
vStart = N.array([vLinspace, -vOnes, vZeros]).astype('float32')
vEnd = N.array([vLinspace, vOnes, vZeros]).astype('float32')
if self.bRotateGrid:
hStart = self.from_homo(N.dot(self.T_stage_arena, self.to_homo(hStart)))
hEnd = self.from_homo(N.dot(self.T_stage_arena, self.to_homo(hEnd)))
vStart = self.from_homo(N.dot(self.T_stage_arena, self.to_homo(vStart)))
vEnd = self.from_homo(N.dot(self.T_stage_arena, self.to_homo(vEnd)))
self.bRotateGrid = False
widthGridline = 1
(hStartProjected,jacobian) = cv2.projectPoints(hStart.transpose(),
rvec,
tvec,
self.K,
self.D)
(hEndProjected,jacobian) = cv2.projectPoints(hEnd.transpose(),
rvec,
tvec,
self.K,
self.D)
(vStartProjected,jacobian) = cv2.projectPoints(vStart.transpose(),
rvec,
tvec,
self.K,
self.D)
(vEndProjected,jacobian) = cv2.projectPoints(vEnd.transpose(),
rvec,
tvec,
self.K,
self.D)
for iLine in range(self.nRows):
cv2.line(img, tuple(hStartProjected[iLine,:][0].astype('int32')), tuple(hEndProjected[iLine,:][0].astype('int32')), cv.CV_RGB(self.colorMax,0,0), widthGridline)
for iLine in range(self.nCols):
cv2.line(img, tuple(vStartProjected[iLine,:][0].astype('int32')), tuple(vEndProjected[iLine,:][0].astype('int32')), cv.CV_RGB(self.colorMax,0,0), widthGridline)
def cube(img):
#img_in = cv2.imread("Picture 27.jpg")
#img = cv2.resize(img_in,None,fx=0.5, fy=0.5, interpolation = cv2.INTER_CUBIC)
#cv2.imshow('img',img)
gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
#cv2.imshow('gray',gray)
ret, corners = cv2.findChessboardCorners(gray, (8,7),None)
# print ret,corners
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
objp = np.zeros((7*8,3), np.float32)
objp[:,:2] = np.mgrid[0:8,0:7].T.reshape(-1,2)
#axis = np.float32([[3,0,0], [0,3,0], [0,0,-3]]).reshape(-1,3)
axis = np.float32([[0,0,0], [0,3,0], [3,3,0], [3,0,0],
[0,0,-3],[0,3,-3],[3,3,-3],[3,0,-3] ])
if ret == True:
cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
# Find the rotation and translation vectors.
rvecs, tvecs, inliers = cv2.solvePnPRansac(objp, corners, mtx, dist)
# project 3D points to image plane
imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)
#print imgpts
img = draw2(img,corners,imgpts)
return img
def update(self, time, ims):
"""Update calibration (transforms, markers, and times)."""
if time in self.times: return
N = len(self.fridge_config.cameras.items())
if len(ims) != N: raise Exception("ExtrinsicCalibration update() expects %d images." % N)
transforms, markers = [], []
for im in ims:
all_detected = self.md.detect(im)
calib = [(m, m_el) for m in all_detected for (m_id, m_el) in self.fridge_config.markers.items()\
if int(m_id)==m.id]
noncalib = [v for v in all_detected if v.id not in map(int, self.fridge_config.markers.keys())]
cur_transform, cur_markers = None, noncalib
for combo in [w for v in xrange(len(calib),2,-1) for w in itc(calib,v)]:
combo_markers, marker_els = zip(*combo)
combo_corners = np.array([v.corners for v in combo_markers]).reshape(-1,2)
pts = np.array([v.T_self_in_world.dot(v.points_in_self)[:3, :].T \
for v in marker_els]).reshape(-1,3)
r, rv, tv = cv2.solvePnP(pts, combo_corners, CAMERA_CMAT, CAMERA_DCOEFFS)
ip = cv2.projectPoints(pts, rv, tv, CAMERA_CMAT, CAMERA_DCOEFFS)[0].reshape(-1,2)
perror = np.linalg.norm((ip-combo_corners)) / ip.shape[0]
if perror < 5:
R = cv2.Rodrigues(rv)[0]
T_world_in_camera = getTransformFromRt(R, tv) # since marker positions in world coords
cur_markers = list(combo_markers) + noncalib
cur_transform = T_world_in_camera
break
transforms.append(cur_transform)
markers.append(cur_markers)
self.transforms.append(transforms)
self.markers.append(markers)
self.times.append(time)
def carve(self, camera, camera_pose, image_mask):
camera_inv = camera_pose.Inverse()
out_points = np.zeros((1, 3, 1), dtype="float32")
cam = np.array(camera.camera_matrix)
dist = np.array(camera.distortion_coefficients)
for ix in range(0, self.side):
for iy in range(0, self.side):
for iz in range(0, self.side):
p = self.getCoordinates(np.array([ix, iy, iz]))
point = PyKDL.Frame(PyKDL.Vector(p[0], p[1], p[2]))
object_to_camera = camera_inv * point
cRvec, cTvec = transformations.KDLToCv(object_to_camera)
point2d, _ = cv2.projectPoints(
out_points, cRvec, cTvec,
cam,
dist
)
point2d = point2d.reshape((2, 1))
i = int(point2d[1])
j = int(point2d[0])
if i < image_mask.shape[0] and j < image_mask.shape[1] and i >= 0 and j >= 0:
if image_mask[i, j] < 1:
self._voxels[
ix, iy, iz] = self._voxels[ix, iy, iz] + 1
import scipy.io
scipy.io.savemat("provola.mat", mdict={
'out': self._voxels}, oned_as='row')
def assign_points_thermal_data(dataset_name, cam_index, thermal_cameras, K_thermal,d_thermal, cameras_top, points_3d):
pic_index, rvec, tvec = thermal_cameras[cam_index]
visible_point_indices = np.array(cameras_top[cam_index][2])
print visible_point_indices.shape
visible_points = np.reshape(points_3d[visible_point_indices,:3],(-1,1,3))
# print visible_point_indices
img_points, _ = cv2.projectPoints(visible_points, rvec, tvec, K_thermal, d_thermal)
img_points = np.squeeze(img_points)
img_points = np.array([[int(u),int(v)] for u,v in img_points])
# print img_points
valid = np.nonzero(np.array([u>=0 and v>=0 and u<640 and v<480 for u,v in img_points]))[0].astype(np.int)
print valid.shape
thermal_img = cv2.imread(config.DATASET_PATH + dataset_name +
'/data/thermal'+str(pic_index)+'.jpg')
# thermal_img_vis = thermal_img[:]
# for u,v, in img_points:
# thermal_img_vis[v,u] = [255,0,0]
# cv2.imshow(str(pic_index),thermal_img_vis)
# cv2.waitKey()
points_thermal = np.array([thermal_img[v,u] for u,v in img_points if u>=0 and v>=0 and u<640 and v<480])
# points_thermal = np.array([[255, 255, 255] for u,v in img_points if u>=0 and v>=0 and u<640 and v<480])
return (visible_point_indices[valid], points_thermal)
def reprojection_error_ext(objp, imgp, cameraMatrix, distCoeffs, rvecs, tvecs):
"""
Returns the mean absolute error, and the RMS error of the reprojection
of 3D points "objp" on the images from a camera
with intrinsics ("cameraMatrix", "distCoeffs") and poses ("rvecs", "tvecs").
The original 2D points should be given by "imgp".
See OpenCV's doc about the format of "cameraMatrix", "distCoeffs", "rvec" and "tvec".
"""
mean_error = np.zeros((1, 2))
square_error = np.zeros((1, 2))
n_images = len(imgp)
for i in xrange(n_images):
imgp_reproj, jacob = cv2.projectPoints(
objp[i], rvecs[i], tvecs[i], cameraMatrix, distCoeffs )
error = imgp_reproj.reshape(-1, 2) - imgp[i]
mean_error += abs(error).sum(axis=0) / len(imgp[i])
square_error += (error**2).sum(axis=0) / len(imgp[i])
mean_error = cv2.norm(mean_error / n_images)
square_error = np.sqrt(square_error.sum() / n_images)
return mean_error, square_error
def drawAxis(img, corners, rvecs, tvecs, mtx, dist, scale=1):
axis = np.float32([[1, 0, 0], [0, 1, 0], [0, 0, -1]])
axis = axis * scale
imgpts, _jac = cv2.projectPoints(axis, np.array(rvecs), np.array(tvecs), mtx, dist)
img = draw3DAxisLines(img, corners[0], imgpts)
def get_view_points(self, angle, height):
width = 0.3
z_offset = -0.2
view_points = [
(616, 425, 1),
(469, 350, 1),
(169, 356, 1),
(23, 430, 1),
]
points = []
for pt in view_points:
pt = np.matrix(pt).T # Append a 1!
back_projected = (np.matrix(self.c920_cam).I * pt).A1
print ' one point', back_projected
points.append(back_projected)
objectPoints = map(lambda pt: np.array([pt], np.float32), points)
rvec = angle * np.array((1.0, 0.0, 0.0), np.float32)
tvec = np.array((0.0, 0.0, 0.0), np.float32)
distCoeffs = np.array([0.169985, -0.401052, 0.005058, -0.00463, 0.0], np.float32)
# distCoeffs = np.array([0.0, 0.0, 0.0, 0.0, 0.0])
view_coordinates = []
for i in range(4):
img_point, _ = cv2.projectPoints(objectPoints[i], rvec, tvec, self.c920_cam, distCoeffs)
view_coordinates.append(img_point[0][0])
return np.float32(view_coordinates)
def createExtrinsicsMatrix(self, chessboardImage, camera_matrix):
found, corners = cv2.findChessboardCorners(chessboardImage,self.patternSize)
if not found:
return
objectPoints = np.array([self.objectPoint])
imagePoints = np.array([corners])
imageShape = chessboardImage.shape[:2]
_, _, dist_coefs, rvecs, tvecs = cv2.calibrateCamera(objectPoints, imagePoints, imageShape)
objectPoints = np.asarray(self.objectPoint, np.float64)
imagePoints = np.asarray(corners, np.float64)
camera_matrix = np.asarray(camera_matrix, np.float64)
dist_coefs = np.array([0, 0, 0, 0], np.float64)
found, rvecs_new, tvecs_new = cv2.solvePnP(objectPoints, imagePoints, camera_matrix, dist_coefs)
rotationMatrix = cv2.Rodrigues(rvecs_new)[0]
img = cv2.projectPoints(np.array([np.array([0,0,0], np.float64)]), rvecs_new, tvecs_new, camera_matrix, dist_coefs)
print img
extrinsicMatrix = np.zeros((3,4))
extrinsicMatrix[:,:-1] = rotationMatrix
extrinsicMatrix[:,-1:] = tvecs_new
return extrinsicMatrix
def _map_monocular(self,p):
if '3d' not in p['method']:
return None
gaze_point = np.array(p['circle_3d']['normal'] ) * self.gaze_distance + np.array( p['sphere']['center'] )
image_point, _ = cv2.projectPoints( np.array([gaze_point]) , self.rotation_vector, self.translation_vector , self.camera_matrix , self.dist_coefs )
image_point = image_point.reshape(-1,2)
image_point = normalize( image_point[0], self.world_frame_size , flip_y = True)
eye_center = self.toWorld(p['sphere']['center'])
gaze_3d = self.toWorld(gaze_point)
normal_3d = np.dot( self.rotation_matrix, np.array( p['circle_3d']['normal'] ) )
g = { 'norm_pos':image_point,
'eye_center_3d':eye_center.tolist(),
'gaze_normal_3d':normal_3d.tolist(),
'gaze_point_3d':gaze_3d.tolist(),
'confidence':p['confidence'],
'timestamp':p['timestamp'],
'base_data':[p]}
if self.visualizer.window:
self.gaze_pts_debug.append( gaze_3d )
self.sphere['center'] = eye_center #eye camera coordinates
self.sphere['radius'] = p['sphere']['radius']
return g
def drawAxisSystem(img, cameraMatrix, distCoeffs, rvec, tvec, scale=4.):
"""
Draws an axis-system on image "img" of which the camera has
the intrinsics ("cameraMatrix", "distCoeffs") and pose ("rvec", "tvec").
The scale of the axis-system is set by "scale".
See OpenCV's doc about the format of "cameraMatrix", "distCoeffs", "rvec" and "tvec".
"""
# Define world object-points
axis_system_objp = np.array([ [0., 0., 0.], # Origin (black)
[1., 0., 0.], # X-axis (red)
[0., 1., 0.], # Y-axis (green)
[0., 0., 1.] ]) # Z-axis (blue)
axis_system_objp *= scale
# Project the object-points on the camera
imgp_reproj, jacob = cv2.projectPoints(
axis_system_objp, rvec, tvec, cameraMatrix, distCoeffs )
origin, xAxis, yAxis, zAxis = np.rint(imgp_reproj.reshape(-1, 2)).astype(np.int32) # round to nearest int
# If projected origin lays out of the image, don't draw axis-system
if not (0 <= origin[0] < img.shape[1] and 0 <= origin[1] < img.shape[0]):
return img
# Draw the axis-system
line(img, origin, xAxis, rgb(255,0,0), thickness=2, lineType=cv2.CV_AA)
line(img, origin, yAxis, rgb(0,255,0), thickness=2, lineType=cv2.CV_AA)
line(img, origin, zAxis, rgb(0,0,255), thickness=2, lineType=cv2.CV_AA)
circle(img, origin, 4, rgb(0,0,0), thickness=-1) # filled circle, radius 4
circle(img, origin, 5, rgb(255,255,255), thickness=2) # white 'O', radius 5
return img
def undistort(mtx, dist, objpoints, imgpoints, rvecs, tvecs, img):
#read in an image of choice
#usefule for the reading in and segmenting of board to make hough lines easier
h, w = img.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1, (w, h))
# undistort
dst = cv2.undistort(img, mtx, dist, None, newcameramtx)
# crop the image
x, y, w, h = roi
dst = dst[y:y + h, x:x + w]
cv2.imwrite('calibresult.png', dst)
# undistort
mapx, mapy = cv2.initUndistortRectifyMap(mtx, dist, None, newcameramtx, (w, h), 5)
dst = cv2.remap(img, mapx, mapy, cv2.INTER_LINEAR)
# crop the image
x, y, w, h = roi
dst = dst[y:y + h, x:x + w]
cv2.imwrite('calibresult.png', dst)
mean_error = 0
for i in xrange(len(objpoints)):
imgpoints2, _ = cv2.projectPoints(objpoints[i], rvecs[i], tvecs[i], mtx, dist)
error = cv2.norm(imgpoints[i], imgpoints2, cv2.NORM_L2) / len(imgpoints2)
mean_error += error
print "total error: ", mean_error / len(objpoints)
def getProjectedPoints(points_3D, Rvect, Tvect, camera, distVect):
resultPoints = cv2.projectPoints(points_3D, Rvect, Tvect, camera, distVect)
resultPoints2 = [[resultPoints[0][0][0][0], resultPoints[0][0][0][1]],
[resultPoints[0][1][0][0], resultPoints[0][1][0][1]],
[resultPoints[0][2][0][0], resultPoints[0][2][0][1]],
[resultPoints[0][3][0][0], resultPoints[0][3][0][1]]]
return resultPoints2
def getCameraCalibration(image, objp):
global criteria, mtx, dist
axis = np.float32([[3,0,0], [0,3,0], [0,0,-3]]).reshape(-1,3)
#height, width, depth = imgorg.shape
gray = cv2.cvtColor(image,cv2.COLOR_BGR2GRAY)
ret = False
# Find the chess board corners
ret, corners = cv2.findChessboardCorners(gray, (7,7), None)
h,w =image.shape[:2]
# If found, add object points, image points (after refining them)
if ret == True:
#objpoints.append(objp)
cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
#imgpoints.append(corners)
rvecs,tvecs,inliers = cv2.solvePnPRansac(objp, corners, mtx, dist)
imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)
img = draw(image, corners, imgpts)
return img
return image
def projectPoints(radar_data, args):
""" Projects radar points into the camera's frame
Args: radar_data, the output from loadRDR
args, the output from parse_args
Returns: A new numpy array with columns:
[dist, lat_dist, z(guess), l, w, rcs, rel_spd, id, x, y]
"""
params = args["params"]
cam_num = args["cam_num"]
cam = params["cam"][cam_num - 1]
radar_data[:, :3] = calibrateRadarPts(radar_data[:, :3], params["radar"])
pts = radar_data[:, :3]
pts_wrt_cam = pts + cam["displacement_from_l_to_c_in_lidar_frame"]
pts_wrt_cam = np.dot(R_to_c_from_l(cam), pts_wrt_cam.transpose())
(pix, J) = cv2.projectPoints(
pts_wrt_cam.transpose(), np.array([0.0, 0.0, 0.0]), np.array([0.0, 0.0, 0.0]), cam["KK"], cam["distort"]
)
pix = pix.transpose()
pix = np.around(pix[:, 0, :])
pix = pix.astype(np.int32)
# Filter the points to remove points that exist outside the video frame
# This is not a problem for the radar because the cameras see everything
# the radar does
# dist_sqr = np.sum(pts_wrt_cam[0:3, :] ** 2, axis = 0)
# mask = (pix[0,:] > 0) & (pix[1,:] > 0) & \
# (pix[0,:] < 1279) & (pix[1,:] < 959) & \
# (pts_wrt_cam[2,:] > 0) & (dist_sqr > 3)
# Outputs [dist, lat_dist, z(guess), l, w, rcs, rel_spd, id, x, y]
radar_data_projected = np.hstack((radar_data, pix.transpose()))
return radar_data_projected
def DrawOriginAxes(self, cvimage, rvec, tvec):
if self.camerainfo is not None:
self.pointsAxes = N.zeros([4, 3], dtype=N.float32) #cv.CreateMat(4, 3, cv.CV_32FC1)
self.pointsAxes[0][:] = 0.0 # (0,0,0)T origin point, pt[0]
self.pointsAxes[1][0] = self.checker_size # (1,0,0)T point on x-axis, pt[1]
self.pointsAxes[2][1] = self.checker_size # (0,1,0)T point on y-axis, pt[2]
self.pointsAxes[3][2] = self.checker_size # (0,0,1)T point on z-axis, pt[3]
widthAxisLine = 3
(self.pointsAxesProjected,jacobian) = cv2.projectPoints(self.pointsAxes,
rvec,
tvec,
self.K,
self.D
)
# origin point
pt1 = tuple(self.pointsAxesProjected[0][0])
# draw x-axis
pt2 = tuple(self.pointsAxesProjected[1][0])
cv2.line(cvimage, pt1, pt2, cv.CV_RGB(self.colorMax,0,0), widthAxisLine)
# draw y-axis
pt2 = tuple(self.pointsAxesProjected[2][0])
cv2.line(cvimage, pt1, pt2, cv.CV_RGB(0,self.colorMax,0), widthAxisLine)
# draw z-axis
pt2 = tuple(self.pointsAxesProjected[3][0])
cv2.line(cvimage, pt1, pt2, cv.CV_RGB(0,0,self.colorMax), widthAxisLine)
def projectPoints(mbly_data, args, T, R):
""" Projects mobileye points into the camera's frame
Args: mbly_data, the output from loadMblyWindow
args, the output from parse_args
"""
params = args['params']
cam_num = args['cam_num']
cam = params['cam'][cam_num]
# Move points to the lidar FoR
pts_wrt_lidar = T_from_mbly_to_lidar(mbly_data, T, R)
# Move the points to the cam FoR
pts_wrt_cam = pts_wrt_lidar +\
cam['displacement_from_l_to_c_in_lidar_frame']
pts_wrt_cam = np.dot(R_to_c_from_l(cam), pts_wrt_cam.transpose())
# Project the points into the camera space
(pix, J) = cv2.projectPoints(pts_wrt_cam.transpose(),
np.array([0.0, 0.0, 0.0]), np.array([0.0, 0.0, 0.0]),
cam['KK'], cam['distort'])
pix = pix.transpose()
pix = np.around(pix[:, 0, :])
pix = pix.astype(np.int32)
mbly_data_projected = np.hstack((mbly_data, pix.transpose()))
return mbly_data_projected
def draw3dCoordAxis(self, img=None, thickness=8):
'''
draw the 3d coordinate axes into given image
if image == False:
create an empty image
'''
if img is None:
img = self.img
elif img is False:
img = np.zeros(shape=self.img.shape, dtype=self.img.dtype)
else:
img = imread(img)
# project 3D points to image plane:
# self.opts['obj_width_mm'], self.opts['obj_height_mm']
w, h = self.opts['new_size']
axis = np.float32([[0.5 * w, 0.5 * h, 0],
[w, 0.5 * h, 0],
[0.5 * w, h, 0],
[0.5 * w, 0.5 * h, -0.5 * w]])
t, r = self.pose()
imgpts = cv2.projectPoints(axis, r, t,
self.opts['cameraMatrix'],
self.opts['distCoeffs'])[0]
mx = int(img.max())
origin = tuple(imgpts[0].ravel())
cv2.line(img, origin, tuple(imgpts[1].ravel()), (0, 0, mx), thickness)
cv2.line(img, origin, tuple(imgpts[2].ravel()), (0, mx, 0), thickness)
cv2.line(
img, origin, tuple(imgpts[3].ravel()), (mx, 0, 0), thickness * 2)
return img
def render_cube(self, image, points):
# load calibration data
with np.load('webcam_calibration_ouput.npz') as X:
mtx, dist, _, _ = [X[i] for i in ('mtx', 'dist', 'rvecs', 'tvecs')]
# set up criteria, image, points and axis
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
imgp = np.array(points, dtype="float32")
objp = np.array([[0., 0., 0.], [1., 0., 0.],
[1., 1., 0.], [0., 1., 0.]], dtype="float32")
axis = np.float32([[0, 0, 0], [0, 1, 0], [1, 1, 0], [1, 0, 0],
[0, 0, -1], [0, 1, -1], [1, 1, -1], [1, 0, -1]])
# project 3D points to image plane
cv2.cornerSubPix(gray, imgp, (11, 11), (-1, -1), criteria)
rvecs, tvecs, _ = cv2.solvePnPRansac(objp, imgp, mtx, dist)
imgpts, _ = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)
# draw cube
self._draw_cube(image, imgpts)
return image
def PointsMask(pos_wrt_camera, Camera):
if False:#'distort' in Camera:
(vpix, J) = projectPoints(pts_wrt_camera.transpose(), np.array([0.0,0.0,0.0]), np.array([0.0,0.0,0.0]), Camera['KK'], Camera['distort'])
else:
vpix = dot(Camera['KK'], divide(pos_wrt_camera, pos_wrt_camera[2,:]))
vpix = around(vpix).astype(np.int32)
return vpix
def plot_circle_centers(board_directories, save_path, world_coords, image_coords, rvec, tvec, cam_mat, dist_coeffs):
start = 0
end = 44
for directory in board_directories:
img = cv2.imread(os.path.join(directory, 'cam1_frame_1.bmp'), 1)
dist_world_coords = world_coords[start:end, :]
if start == 0:
dist = 50.0
elif start == 44:
dist = 100.0
elif start == 88:
dist = 450.0
screen_edges = np.array([[-121/2.0, 68/2.0, dist],
[121/2.0, 68/2.0, dist],
[-121/2.0, -68/2.0, dist],
[121/2.0, -68/2.0, dist]])
dist_world_coords = np.concatenate((dist_world_coords, screen_edges))
camera_coords, jac = cv2.projectPoints(objectPoints=dist_world_coords,rvec=rvec,tvec=tvec,cameraMatrix=cam_mat,distCoeffs=dist_coeffs)
camera_coords = camera_coords.squeeze()
for coords in camera_coords:
cv2.circle(img, tuple(coords), 3, (0,0,255), -1)
for coords in image_coords[start:end]:
cv2.circle(img, tuple(coords), 5, (180,255,0), 2)
cv2.imwrite(os.path.join(save_path, 'reprojection_'+str(int(dist))+'.png'), img)
start += 44
end += 44
def construct_test_image( az_rot, pitch_rot, pos_x, pos_y, pos_z ):
projected = {}
rectangles = []
y_axis = np.array([0,1,0])
az_rot_matrix = rotation_matrix(y_axis,az_rot)
x_axis = np.array([1,0,0])
el_rot_matrix = rotation_matrix(x_axis,pitch_rot)
sum_rot_matrix = np.dot(el_rot_matrix,az_rot_matrix)
for k, a in test_locs.iteritems():
p = cv2.projectPoints(np.array([a + [-pos_x,-pos_y,-pos_z]]), sum_rot_matrix, np.float64([0,0,0]), cameraMatrix, distCoeff)[0][0][0]
if ( 0 <= p[0] < 319 ) and ( 0 <= p[1] < 239 ):
projected[k] = p
if ('ml-ul' in projected) and ('ml-ll' in projected) and ('ml-ur' in projected) and ('ml-lr' in projected):
rectangles.append( get_sides( projected['ml-ul'], projected['ml-ll'], projected['ml-ur'], projected['ml-lr'] ) )
if ('mr-ul' in projected) and ('mr-ll' in projected) and ('mr-ur' in projected) and ('mr-lr' in projected):
rectangles.append( get_sides( projected['mr-ul'], projected['mr-ll'], projected['mr-ur'], projected['mr-lr'] ) )
if ('bt-ul' in projected) and ('bt-ll' in projected) and ('bt-ur' in projected) and ('bt-lr' in projected):
rectangles.append( get_sides( projected['bt-ul'], projected['bt-ll'], projected['bt-ur'], projected['bt-lr'] ) )
if ('tp-ul' in projected) and ('tp-ll' in projected) and ('tp-ur' in projected) and ('tp-lr' in projected):
rectangles.append( get_sides( projected['tp-ul'], projected['tp-ll'], projected['tp-ur'], projected['tp-lr'] ) )
return rectangles
def update(self,frame,events):
gaze_pts = []
for p in events['pupil_positions']:
if p['method'] == '3d c++' and p['confidence'] > self.g_pool.pupil_confidence_threshold:
gaze_point = np.array(p['circle_3d']['normal'] ) * self.gaze_distance + np.array( p['sphere']['center'] )
image_point, _ = cv2.projectPoints( np.array([gaze_point]) , self.rotation_vector, self.translation_vector , self.camera_matrix , self.dist_coefs )
image_point = image_point.reshape(-1,2)
image_point = normalize( image_point[0], (frame.width, frame.height) , flip_y = True)
eye_center = self.toWorld(p['sphere']['center'])
gaze_3d = self.toWorld(gaze_point)
normal_3d = np.dot( self.rotation_matrix, np.array( p['circle_3d']['normal'] ) )
gaze_pts.append({ 'norm_pos':image_point,
'eye_center_3d':eye_center.tolist(),
'gaze_normal_3d':normal_3d.tolist(),
'gaze_point_3d':gaze_3d.tolist(),
'confidence':p['confidence'],
'timestamp':p['timestamp'],
'base':[p]})
if self.visualizer.window:
self.gaze_pts_debug.append( gaze_3d )
self.sphere['center'] = eye_center #eye camera coordinates
self.sphere['radius'] = p['sphere']['radius']
events['gaze_positions'] = gaze_pts
def project_xy(xy_coords, pvec):
# get cubic polynomial coefficients given
#
# f(0) = 0, f'(0) = alpha
# f(1) = 0, f'(1) = beta
alpha, beta = tuple(pvec[CUBIC_IDX])
poly = np.array([
alpha + beta,
-2*alpha - beta,
alpha,
0])
xy_coords = xy_coords.reshape((-1, 2))
z_coords = np.polyval(poly, xy_coords[:, 0])
objpoints = np.hstack((xy_coords, z_coords.reshape((-1, 1))))
image_points, _ = cv2.projectPoints(objpoints,
pvec[RVEC_IDX],
pvec[TVEC_IDX],
K, np.zeros(5))
return image_points
def on_frame(self, vis):
match = self.match_frames()
if match is None:
return
w, h = getsize(self.frame)
p0, p1, H = match
for (x0, y0), (x1, y1) in zip(np.int32(p0), np.int32(p1)):
cv2.line(vis, (x0+w, y0), (x1, y1), (0, 255, 0))
x0, y0, x1, y1 = self.ref_rect
corners0 = np.float32([[x0, y0], [x1, y0], [x1, y1], [x0, y1]])
img_corners = cv2.perspectiveTransform(corners0.reshape(1, -1, 2), H)
cv2.polylines(vis, [np.int32(img_corners)], True, (255, 255, 255), 2)
corners3d = np.hstack([corners0, np.zeros((4, 1), np.float32)])
fx = 0.9
K = np.float64([[fx*w, 0, 0.5*(w-1)],
[0, fx*w, 0.5*(h-1)],
[0.0,0.0, 1.0]])
dist_coef = np.zeros(4)
ret, rvec, tvec = cv2.solvePnP(corners3d, img_corners, K, dist_coef)
verts = ar_verts * [(x1-x0), (y1-y0), -(x1-x0)*0.3] + (x0, y0, 0)
verts = cv2.projectPoints(verts, rvec, tvec, K, dist_coef)[0].reshape(-1, 2)
for i, j in ar_edges:
(x0, y0), (x1, y1) = verts[i], verts[j]
cv2.line(vis, (int(x0), int(y0)), (int(x1), int(y1)), (255, 255, 0), 2)
undistorted = undistorted[y:y + h, x:x + w]
cv2.imwrite('./generated/resultl.jpg', undistorted)
imgr = cv2.imread('./images/chessboard-5r.jpg')
h, w = imgr.shape[:2]
newcameramtxr, roir = cv2.getOptimalNewCameraMatrix(mtxr, distr, (w, h), 1,
(w, h))
undistorted = cv2.undistort(imgr, mtxr, distr, None, newcameramtxr)
x, y, w, h = roir
undistorted = undistorted[y:y + h, x:x + w]
cv2.imwrite('./generated/resultr.jpg', undistorted)
mean_error = 0
for i in range(len(objpoints)):
imgpoints2, _ = cv2.projectPoints(objpoints[i], rvecsl[i], tvecsl[i], mtxl,
distl)
error = cv2.norm(imgpointsl[i], imgpoints2, cv2.NORM_L2) / len(imgpoints2)
mean_error += error
print("total error(left): {}".format(mean_error / len(objpoints)))
mean_error = 0
for i in range(len(objpoints)):
imgpoints2, _ = cv2.projectPoints(objpoints[i], rvecsr[i], tvecsr[i], mtxr,
distr)
error = cv2.norm(imgpointsr[i], imgpoints2, cv2.NORM_L2) / len(imgpoints2)
mean_error += error
print("total error(right): {}".format(mean_error / len(objpoints)))
# Stereo stuff
error, mtxl, distl, mtxr, distr, R, T, E, F = cv2.stereoCalibrate(
objpoints,
def get3D(c1, c2, mask1, mask2, glass, _img1=None, _img2=None, drawCentroid=False, drawDimensions=False): img1 = copy.deepcopy(_img1) img2 = copy.deepcopy(_img2) centr1 = getCentroid(c1, mask1) centr2 = getCentroid(c2, mask2) if centr1 is not None and centr2 is not None: glass.centroid = triangulate(c1, c2, centr1, centr2)[:-1].reshape(-1,3) # Draw centroid if drawCentroid: # Draw 2D centroid of tracking mask #cv2.circle(img1, tuple(centr1.astype(int)), 10, (0,128,0), -1) #cv2.circle(img2, tuple(centr2.astype(int)), 10, (0,128,0), -1) # Draw 3D centroid projected to image point1, _ = cv2.projectPoints(glass.centroid, c1.extrinsic['rgb']['rvec'], c1.extrinsic['rgb']['tvec'], c1.intrinsic['rgb'], c1.distCoeffs) point2, _ = cv2.projectPoints(glass.centroid, c2.extrinsic['rgb']['rvec'], c2.extrinsic['rgb']['tvec'], c2.intrinsic['rgb'], c2.distCoeffs) point1 = point1.squeeze().astype(int) point2 = point2.squeeze().astype(int) cv2.circle(img1, tuple(point1), 6, (128,0,0), -1) cv2.circle(img2, tuple(point2), 6, (128,0,0), -1) # Draw height and width lines if drawDimensions: # Get top/bottom points top = copy.deepcopy(glass.centroid) bottom = copy.deepcopy(glass.centroid) top[0,2] += glass.h/2 bottom[0,2] -= glass.h/2 topC1, _ = cv2.projectPoints(top, c1.extrinsic['rgb']['rvec'], c1.extrinsic['rgb']['tvec'], c1.intrinsic['rgb'], c1.distCoeffs) bottomC1, _ = cv2.projectPoints(bottom, c1.extrinsic['rgb']['rvec'], c1.extrinsic['rgb']['tvec'], c1.intrinsic['rgb'], c1.distCoeffs) topC2, _ = cv2.projectPoints(top, c2.extrinsic['rgb']['rvec'], c1.extrinsic['rgb']['tvec'], c2.intrinsic['rgb'], c2.distCoeffs) bottomC2, _ = cv2.projectPoints(bottom, c2.extrinsic['rgb']['rvec'], c2.extrinsic['rgb']['tvec'], c2.intrinsic['rgb'], c2.distCoeffs) topC1 = topC1.squeeze().astype(int) bottomC1 = bottomC1.squeeze().astype(int) topC2 = topC2.squeeze().astype(int) bottomC2 = bottomC2.squeeze().astype(int) # Get rigth/left points right = copy.deepcopy(glass.centroid) left = copy.deepcopy(glass.centroid) right[0,0] += glass.w/2 left[0,0] -= glass.w/2 rightC1, _ = cv2.projectPoints(right, c1.extrinsic['rgb']['rvec'], c1.extrinsic['rgb']['tvec'], c1.intrinsic['rgb'], c1.distCoeffs) leftC1, _ = cv2.projectPoints(left, c1.extrinsic['rgb']['rvec'], c1.extrinsic['rgb']['tvec'], c1.intrinsic['rgb'], c1.distCoeffs) rightC2, _ = cv2.projectPoints(right, c2.extrinsic['rgb']['rvec'], c2.extrinsic['rgb']['tvec'], c2.intrinsic['rgb'], c2.distCoeffs) leftC2, _ = cv2.projectPoints(left, c2.extrinsic['rgb']['rvec'], c2.extrinsic['rgb']['tvec'], c2.intrinsic['rgb'], c2.distCoeffs) rightC1 = rightC1.squeeze().astype(int) leftC1 = leftC1.squeeze().astype(int) rightC2 = rightC2.squeeze().astype(int) leftC2 = leftC2.squeeze().astype(int) cv2.line(img1, tuple(topC1), tuple(bottomC1), (128,0,0), 2) cv2.line(img1, tuple(rightC1), tuple(leftC1), (128,0,0), 2) cv2.line(img2, tuple(topC2), tuple(bottomC2), (128,0,0), 2) cv2.line(img2, tuple(rightC2), tuple(leftC2), (128,0,0), 2) return glass, img1, img2
def distort(points):
points = points.reshape((-1, 2))
points_3d = numpy.zeros((len(points), 3))
points_3d[:, 0:2] = points
return cv2.projectPoints(points_3d, (0, 0, 0), (0, 0, 0), cameraMatrix,
distCoeffs)[0]
def process(self, curr_frame):
"""
Track features from previous frame to current frame, if a parallax exceding a threshold has been
reached, determine change in pose between previous keyframe and current frame and update the state
accordingly. Then triangulate the tracked features in current frame and previous key to achieve 3D
observations of tracked features.
"""
# Preprocess curr frame
curr_frame = self.preprocess(curr_frame)
# First iteration
if self.prev_frame is None:
# Detect first features and create first keyframe
curr_points = self.detect_features(curr_frame)
self.keyframe_points = curr_points
else:
# Track features detected in first image to second image
curr_points, status,_ = cv.calcOpticalFlowPyrLK(self.prev_frame, curr_frame, self.prev_points, None, **self.lk_params)
if (np.count_nonzero(status) < self.points_thresh):
# Detect new features (or add them on to existing ones??)
print('Detecting New Features, Ran Out', np.count_nonzero(status))
curr_points = self.detect_features(curr_frame)
self.keyframe_points = curr_points
else:
# Only keep the points in the images tracked succesfully from the first to the second image
# Store in seperate variable to preserve shapes of prev, curr, keyframe points i.e (n, 1, 2) instead of (n,2)
points_prev = self.prev_points[status == 1]
points_curr = curr_points[status == 1]
points_key = self.keyframe_points[status == 1]
# Update prev, curr, and keyframe points, reshaped
self.prev_points = points_prev.reshape((points_prev.shape[0], 1, 2))
curr_points = points_curr.reshape((points_curr.shape[0], 1, 2))
self.keyframe_points = points_key.reshape((points_key.shape[0], 1, 2))
# Calculate the mean distance between all corresponding points in each image
# This is a rough estimation of how much motion has been captured by the frame sequence
self.parallax = self.parallax + np.mean(np.linalg.norm(points_prev-points_curr, axis=1))
#If we have approximately moved enough for an accurate pose estimate
if (self.parallax > self.parallax_thresh):
# Reset self.parallax
self.parallax = 0.0
# Recover change in pose between last keyframe and curr points
E,_ = cv.findEssentialMat(points_key, points_curr, self. K)
_, R, t,_ = cv.recoverPose(E, points_key, points_curr, self.K)
if self.R_net is None and self.t_net is None:
self.R_net = np.eye(3)
self.t_net = np.zeros((3,1))
# Store values of self.state before updating it (to calculate reprojection of 3D points)
R_net_prev = self.R_net.copy()
t_net_prev = self.t_net.copy()
# Projection matrix from origin to last keyframe
P_key = np.matmul(self.K, np.hstack((self.R_net, self.t_net)))
# Update self.state using only pose estimation from essential matrix
self.t_net = self.t_net + np.dot(self.R_net, t)
self.R_net = np.matmul(R, self.R_net)
self.state = self.t_net.flatten()
self.state_data.append(self.state.copy())
# Projection matrix from last keyframe to curr frame
P = np.matmul(self.K, np.hstack((self.R_net, self.t_net)))
# Triangulate from keyframe points to curr points
points_hom = cv.triangulatePoints(P_key, P, points_key.T.astype(float), points_curr.T.astype(float))
# Convert homogenous coordinates to 3D coordinates
points_hom[0] = points_hom[0]/points_hom[3]
points_hom[1] = points_hom[1]/points_hom[3]
points_hom[2] = points_hom[2]/points_hom[3]
points_3d = points_hom[:3].T
# Check reprojection error of 3D points onto keyframe
R_vec_prev,_ = cv.Rodrigues(R_net_prev)
points_key_reproj,_ = cv.projectPoints(points_3d, R_vec_prev, t_net_prev, self.K, np.zeros((4,)) )
points_key_reproj = points_key_reproj.reshape((points_key_reproj.shape[0], 2))
print("Reprojection error of triangulated 3D points onto last keyframe: ", np.linalg.norm(points_key-points_key_reproj))
# Plot 3D points at every keyframe creation
# ax.scatter(points_3d.T[0], points_3d.T[1], points_3d.T[2])
# plt.pause(10)
# Solve for R, t between keyframe and curr frame using PNP
# _,R_vec, t, inliers = cv.solvePnPRansac(points_3d, points_curr, self.K, np.zeros((4,)))
# Make keyframe newly detected points
curr_points = self.detect_features(curr_frame)
self.keyframe_points = curr_points
# Draw features on orig image
for p in curr_points:
x,y = p.ravel()
cv.circle(curr_frame, (x,y), 3, 255, -1)
# Set prev values to curr values
self.prev_frame = curr_frame
self.prev_points = curr_points
return curr_frame
def main():
files = glob.glob('./Calibration_Imgs/*.jpg')
# given, reference block size is 21.5 units
# note XY refer to horizontal and vertical of image (not to the axis marked on the reference image)
wCordXY = np.array([[21.5, 21.5],
[21.5,21.5*9],
[21.5*6, 21.5*9],
[21.5*6,21.5]], dtype= np.float32)
allImgs= []
allImgOrg = []
for File in files:
img = cv2.imread(File)
imgOrg =img.copy()
allImgOrg.append(imgOrg)
allImgs.append(img)
allH, all_corn, allcr = findHomo(allImgs, wCordXY)
vmat, b = VMat(allH)
A = find_K(b)
# allRT = getRT(allH, A)
K = [0,0]
print('Initial estimate for Camera Intrinsic Matrix -')
print(A)
print('')
param = np.array([A[0,0],0.0,A[0,2],A[1,1],A[1,2],0.0,0.0])
allW_xy = []
for i in range(6):
for j in range(9):
allW_xy.append(np.array([21.5*(i+1), 21.5*(j+1),0,1]))
allW_xy = np.array(allW_xy, dtype =np.float64).reshape(54,4)
res = least_squares(fun, x0=param, method='lm', args=(all_corn, allW_xy, allH))
print('Minimization convergance status= ', res.success)
print('')
A_opt = np.zeros([3,3], dtype= np.float)
A_opt[0,0] = res.x[0]
A_opt[0,1] = res.x[1]
A_opt[0,2] = res.x[2]
A_opt[1,1] = res.x[3]
A_opt[1,2] = res.x[4]
A_opt[2,2] = 1
k_opt = [res.x[5], res.x[6]]
print('Optimised Distortion Coefficients -')
print(k_opt)
print('')
print('Optimized Camera Intrinsic Matrix -')
print(A_opt)
print('')
dist_opt = np.array([k_opt[0],k_opt[1],0,0,0])
allRT_opt = getRT(allH, A_opt)
err= []
allW_xy1 = np.array(allW_xy[:,:3], dtype =np.float32)
for i in xrange(len(allH)):
R = allRT_opt[i][:,0:3]
rvec,_ = cv2.Rodrigues(R)
t = allRT_opt[i][:,3]
imgpoints2, _ = cv2.projectPoints(allW_xy1, rvec, t, A_opt,dist_opt)
diff = allcr[i] - imgpoints2
error = np.sum(np.linalg.norm(diff,axis=1))/len(imgpoints2)
err.append(error)
err =np.array(err)
rejErr = np.mean(err)
print('Re-projection error -')
print(rejErr)
print('')
print('Generating undistorted images')
count =1
for image in allImgOrg:
undistImg = cv2.undistort(image,A_opt,dist_opt)
cv2.imwrite('./undist_output/'+str(count)+'_undistort.png',undistImg)
count+=1
img = cv2.line(img, corner, tuple(imgpts[1].ravel()), (0, 255, 0), 5)
img = cv2.line(img, corner, tuple(imgpts[2].ravel()), (0, 0, 255), 5)
return img
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
objp = np.zeros((6 * 7, 3), np.float32)
objp[:, :2] = np.mgrid[0:7, 0:6].T.reshape(-1, 2)
axis = np.float32([[3, 0, 0], [0, 3, 0], [0, 0, -3]]).reshape(-1, 3)
for fname in glob.glob('calib_images/left03.jpg'):
img = cv2.imread(fname)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, (7, 6), None)
if ret == True:
corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1),
criteria)
# Find the rotation and translation vectors.
_, rvecs, tvecs, inliers = cv2.solvePnPRansac(objp, corners2, mtx,
dist)
# project 3D points to image plane
imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)
img = draw(img, corners2, imgpts)
cv2.imshow('img', img)
k = cv2.waitKey(0) & 0xff
cv2.destroyAllWindows()
def triangulate(self, cal, det1, det2):
"""
Assuming, det1 matches det2, we determine 3d-coordinates of each
detection and measure the reprojection error.
References:
http://answers.opencv.org/question/117141
https://gist.github.com/royshil/7087bc2560c581d443bc
https://stackoverflow.com/a/29820184/887074
Doctest:
>>> # Rows are detections in img1, cols are detections in img2
>>> from viame.processes.camtrawl.algos import *
>>> from viame.processes.camtrawl.demo import *
>>> detections1, detections2, cal = demodata_detections(target_step='triangulate', target_frame_num=6)
>>> det1, det2 = detections1[0], detections2[0]
>>> self = FishStereoMeasurments()
>>> assignment, assign_data, cand_errors = self.triangulate(cal, det1, det2)
"""
logger.debug('triangulate')
_debug = 0
if _debug:
# Use 4 corners and center to ensure matrix math is good
# (hard to debug when ndims == npts, so make npts >> ndims)
pts1 = np.vstack([det1.box_points(), det1.oriented_bbox().center])
pts2 = np.vstack([det2.box_points(), det2.oriented_bbox().center])
else:
# Use only the corners of the bbox
pts1 = det1.box_points()[[0, 2]]
pts2 = det2.box_points()[[0, 2]]
# Move into opencv point format (num x 1 x dim)
pts1_cv = pts1[:, None, :]
pts2_cv = pts2[:, None, :]
# Grab camera parameters
K1, K2 = cal.intrinsic_matrices()
kc1, kc2 = cal.distortions()
rvec1, tvec1, rvec2, tvec2 = cal.extrinsic_vecs()
# Make extrincic matrices
R1 = cv2.Rodrigues(rvec1)[0]
R2 = cv2.Rodrigues(rvec2)[0]
T1 = tvec1[:, None]
T2 = tvec2[:, None]
RT1 = np.hstack([R1, T1])
RT2 = np.hstack([R2, T2])
# Undistort points
# This puts points in "normalized camera coordinates" making them
# independent of the intrinsic parameters. Moving to world coordinates
# can now be done using only the RT transform.
unpts1_cv = cv2.undistortPoints(pts1_cv, K1, kc1)
unpts2_cv = cv2.undistortPoints(pts2_cv, K2, kc2)
# note: trinagulatePoints docs say that it wants a 3x4 projection
# matrix (ie K.dot(RT)), but we only need to use the RT extrinsic
# matrix because the undistorted points already account for the K
# intrinsic matrix.
world_pts_homog = cv2.triangulatePoints(RT1, RT2, unpts1_cv, unpts2_cv)
world_pts = from_homog(world_pts_homog)
# Compute distance between 3D bounding box points
if _debug:
corner1, corner2 = world_pts.T[[0, 2]]
else:
corner1, corner2 = world_pts.T
# Length is in milimeters
fishlen = np.linalg.norm(corner1 - corner2)
# Reproject points
world_pts_cv = world_pts.T[:, None, :]
proj_pts1_cv = cv2.projectPoints(world_pts_cv, rvec1, tvec1, K1,
kc1)[0]
proj_pts2_cv = cv2.projectPoints(world_pts_cv, rvec2, tvec2, K2,
kc2)[0]
# Check error
err1 = ((proj_pts1_cv - pts1_cv)[:, 0, :]**2).sum(axis=1)
err2 = ((proj_pts2_cv - pts2_cv)[:, 0, :]**2).sum(axis=1)
errors = np.hstack([err1, err2])
# Get 3d points in each camera's reference frame
# Note RT1 is the identity and RT are 3x4, so no need for `from_homog`
# Return points in with shape (N,3)
pts1_3d = RT1.dot(to_homog(world_pts)).T
pts2_3d = RT2.dot(to_homog(world_pts)).T
return pts1_3d, pts2_3d, errors, fishlen
def __init__(self):
# Variable Initialization
self.grid_size = 5 # grid_size and grid_center are related
self.grid_center = 2
self.bridge = CvBridge() # ROS/Gazebo to cv2 converter
self.object_points_index = np.zeros(
(self.grid_size * self.grid_size, 3),
np.float32) # Form object points
self.object_points_index[:, :2] = np.mgrid[0:self.grid_size,
0:self.grid_size].T.reshape(
-1,
2) # FOrm object points
self.number_of_cal_images = 0 # Track number of calibration images taken
self.object_points = [] # Array for storing object points
self.image_points = [] # Array for storing image points
self.axis = np.float32(
[[3, 0, 0], [0, 3, 0], [0, 0, -3],
[self.grid_center, self.grid_center,
0]]).reshape(-1, 3) # Axis for determining point location
self.transform = np.zeros((4, 4))
self.center_from_vehicle = np.zeros((4, 1))
self.center_in_image = np.zeros((4, 1))
self.command_timer_start = rospy.get_time()
# Configuration Parameters
self.Hertz = 20 # frequency of while loop
criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001
) # Termination Criteria
self.cal_images_needed = 10 # Number of Calibration images needed
self.gain = 1
PID_x = [
0.02, 0.5, 3
] # PID Controller Tuning Values (latitude) TODO Tune Controller
PID_y = [
0.02, 0.5, 3
] # PID Controller Tuning Values (longitude) TODO Tune Controller
self.decent_rate = -0.4
# Subscribers
rospy.Subscriber("/down_cam/camera/image",
Image,
self.callbackCamera,
queue_size=1) # Downward Facing Camera Subscriber
# Publishers
vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
# Messages
vel_msg = Twist()
vel_msg.linear.z = self.decent_rate
vel_msg.angular.x = 0
vel_msg.angular.y = 0
vel_msg.angular.z = 0
# Loop Timing
rate = rospy.Rate(self.Hertz)
time.sleep(2) # allows the callback functions to populate variables
print("Commencing Camera Calibration")
while not rospy.is_shutdown():
# Process Image
gray_image = cv.cvtColor(self.image, cv.COLOR_BGR2GRAY)
# Check for corners in the image
ret, corners = cv.findChessboardCorners(
gray_image, (self.grid_size, self.grid_size), None)
# If the corners are seen the image can be processed
if ret:
corners2 = cv.cornerSubPix(gray_image, corners, (11, 11),
(-1, -1), criteria)
if self.number_of_cal_images <= self.cal_images_needed:
self.object_points.append(self.object_points_index)
self.image_points.append(corners)
# Update Calibration Tracker
self.number_of_cal_images = self.number_of_cal_images + 1
print(
"Need {} more images to complete calibration.".format(
self.cal_images_needed -
self.number_of_cal_images))
# Draw Calibration Image
cv.drawChessboardCorners(self.image,
(self.grid_size, self.grid_size),
corners2, ret)
ret, mtx, dist, rvecs, tvecs = cv.calibrateCamera(
self.object_points, self.image_points,
gray_image.shape[::-1], None, None)
# If the calibration has been done at least once
if self.number_of_cal_images > 0:
ret, rvecs, tvecs = cv.solvePnP(self.object_points_index,
corners2, mtx, dist)
# rvecs and tvecs are 3x1 arrays
# Determine Image Points
image_points, jacobian = cv.projectPoints(
self.axis, rvecs, tvecs, mtx, dist)
annotated_image = self.draw(self.image, corners2,
image_points)
# Transform
rotation = cv.Rodrigues(
rvecs) # 3x3 rotation matrix with 9x3 jacobian
tvecs = np.transpose(tvecs)
self.transform[0:3, 0:3] = rotation[0]
self.transform[0:3, 3] = tvecs
self.transform = np.linalg.pinv(self.transform)
formated_center = np.transpose(self.axis[3, :])
self.center_in_image[0:3, 0] = formated_center
self.center_in_image[3, 0] = 1
self.center_from_vehicle = self.transform.dot(
self.center_in_image)
if (self.number_of_cal_images > self.cal_images_needed):
x_pid = PID(PID_x[0],
PID_x[1],
PID_x[2],
setpoint=-self.center_from_vehicle[0],
sample_time=1 / self.Hertz,
output_limits=(-2, 2))
y_pid = PID(PID_y[0],
PID_y[1],
PID_y[2],
setpoint=self.center_from_vehicle[1],
sample_time=1 / self.Hertz,
output_limits=(-2, 2))
vel_msg.linear.x = x_pid(self.grid_center)
vel_msg.linear.y = y_pid(self.grid_center)
vel_pub.publish(vel_msg)
elif self.number_of_cal_images > self.cal_images_needed:
vel_msg.linear.x = 0
vel_msg.linear.y = 0
vel_pub.publish(vel_msg)
cv.imshow("Processed Image", self.image)
cv.waitKey(3)
rate.sleep()
cv.destroyAllWindows()
# random.shuffle(tmp)
# objpoints_L, imgpoints_L, objpoints_R, imgpoints_R = zip(*tmp)
# objpoints_L = objpoints_L[:valid_num]
# imgpoints_L = imgpoints_L[:valid_num]
# objpoints_R = objpoints_R[:valid_num]
# imgpoints_R = imgpoints_R[:valid_num]
ret, mtxL, distcoeffL, rvecsL, tvecsL = cv2.calibrateCamera(
objpoints_L, imgpoints_L, size, None, None)
ret, mtxR, distcoeffR, rvecsR, tvecsR = cv2.calibrateCamera(
objpoints_R, imgpoints_R, size, None, None)
print("Valid img count:" + str(count))
error_L = []
for i in range(len(objpoints_L)):
imgpoints2, _ = cv2.projectPoints(objpoints_L[i], rvecsL[i], tvecsL[i],
mtxL, distcoeffL)
error = cv2.norm(imgpoints_L[i], imgpoints2, cv2.NORM_L2) / len(imgpoints2)
error_L.append(error)
print(error_L)
error_R = []
ret, mtxR, distcoeffR, rvecsR, tvecsR = cv2.calibrateCamera(
objpoints_R, imgpoints_R, size, None, None)
for i in range(len(objpoints_R)):
imgpoints2, _ = cv2.projectPoints(objpoints_R[i], rvecsR[i], tvecsR[i],
mtxR, distcoeffR)
error = cv2.norm(imgpoints_R[i], imgpoints2, cv2.NORM_L2) / len(imgpoints2)
error_R.append(error)
print(error_R)
# stereoCalibrate
retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = cv2.stereoCalibrate(
h, w = img1.shape
pts = np.float32([[0, 0], [w / 2, 0], [w - 1, 0], [0, h / 2],
[w - 1, h / 2], [0, h - 1], [w / 2, h - 1],
[w - 1, h - 1]]).reshape(-1, 1, 2)
#print(M)
if (not (M is None)) and M.shape[0] != None:
dst = cv2.perspectiveTransform(pts, M)
else:
dst = []
img2 = cv2.polylines(img2, [np.int32(dst)], True, 255, 3, cv2.LINE_AA)
#print(dst)
try:
pnp = cv2.solvePnPRansac(objectPoints, dst, cameraMatrix, None)
#print(pnp)
dst, j = cv2.projectPoints(objectPoints, pnp[1], pnp[2],
cameraMatrix, None)
print(dst)
print(pnp[2])
img2 = cv2.circle(img2, (int(dst[0][0][0]), int(dst[0][0][1])), 30,
(0, 255, 0), -1) # Top Left
img2 = cv2.circle(img2, (int(dst[2][0][0]), int(dst[2][0][1])), 30,
(0, 255, 255), -1) # Bottom Left
img2 = cv2.circle(img2, (int(dst[5][0][0]), int(dst[5][0][1])), 30,
(0, 0, 255), -1) # Bottom Right
img2 = cv2.circle(img2, (int(dst[7][0][0]), int(dst[7][0][1])), 30,
(255, 255, 0), -1) # Top Right
print("c")
print(roundArray(rotationMatrixToEulerAngles(pnp[1])))
#eulerAngles = rotationMatrixToEulerAngles(cv2.Rodrigues(pnp[1]))
#print(eulerAngles)
def project(self, p3d):
img_pts = np.zeros((len(self.cameras), p3d.shape[0], 2))
for i, c in enumerate(self.cameras):
proj, _ = cv2.projectPoints(p3d, c.rvec, c.tvec, c.K, c.dist)
img_pts[i, :, :] = proj[:, 0, :]
return img_pts
def getPoints(im, detector, predictor):
gray = cv2.cvtColor(im, cv2.COLOR_BGR2GRAY)
rects = detector(gray, 1)
size = im.shape
for (i, rect) in enumerate(rects):
shape = predictor(gray, rect)
shape = face_utils.shape_to_np(shape)
count = 0
for (x,y) in shape :
#cv2.circle(im, (x, y), 1, (0, 0, 255), -1)
if count == 34 :
nose_tip = (x,y)
if count == 9 :
chin = (x,y)
if count == 46 :
leyelcorner = (x,y)
if count == 18 :
reyercorner = (x,y)
if count == 55 :
lmouthcorner = (x,y)
if count == 49 :
rmouthcorner = (x,y)
count += 1
#2D image points. If you change the image, you need to change vector
image_points = np.array([
nose_tip, # Nose tip
chin, # Chin
leyelcorner, # Left eye left corner
reyercorner, # Right eye right corne
lmouthcorner, # Left Mouth corner
rmouthcorner # Right mouth corner
], dtype="double")
# 3D model points.
model_points = np.array([
(0.0, 0.0, 0.0), # Nose tip
(0.0, -330.0, -65.0), # Chin
(-225.0, 170.0, -135.0), # Left eye left corner
(225.0, 170.0, -135.0), # Right eye right corne
(-150.0, -150.0, -125.0), # Left Mouth corner
(150.0, -150.0, -125.0) # Right mouth corner
])
# Camera internals
focal_length = size[1]
center = (size[1]/2, size[0]/2)
camera_matrix = np.array(
[[focal_length, 0, center[0]],
[0, focal_length, center[1]],
[0, 0, 1]], dtype = "double"
)
dist_coeffs = np.zeros((4,1)) # Assuming no lens distortion
(success, rotation_vector, translation_vector) = cv2.solvePnP(model_points, image_points, camera_matrix, dist_coeffs, flags=cv2.SOLVEPNP_ITERATIVE)
# Project a 3D point (0, 0, 1000.0) onto the image plane.
# We use this to draw a line sticking out of the nose
(nose_end_point2D, jacobian) = cv2.projectPoints(np.array([(0.0, 0.0, 1000.0)]), rotation_vector, translation_vector, camera_matrix, dist_coeffs)
p1 = ( int(image_points[0][0]), int(image_points[0][1]))
p2 = ( int(nose_end_point2D[0][0][0]), int(nose_end_point2D[0][0][1]))
# cv2.line(im, p1, p2, (255,0,0), 2)
# cv2.imshow("line", im)
# cv2.waitKey(0)
sign = 0
if p2[0] >= 0 :
sign = -1
else :
sign = 1
return (p1, p2, sign)
point2d = [d["cross2d"] for d in data]
#print point3d
p = Program(point3d, point2d, x0, x0_int)
cam = p.run()
# show reprojection
fx, fy = x0_int[0], x0_int[1]
cx, cy = x0_int[2], x0_int[3]
distCoeffs = (0,0,0,0,0)
tvec = cam[0:3] # x,y,z
rvec = cam[3:6] # rodrigues
# project
cameraMatrix = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])
point2Ds_p, jacobian = cv2.projectPoints(npa(point3d, dtype=np.float), rvec, tvec, cameraMatrix, distCoeffs)
point2Ds_p_nd, jacobian = cv2.projectPoints(npa(point3d, dtype=np.float), rvec, tvec, cameraMatrix, (0.,0.,0.,0.,0.))
for i, d in enumerate(data):
image_viz = cv2.imread(save_dir + d['pic_path'])
pt_int = tuple([int(round(p)) for p in d['cross2d']])
cv2.line(image_viz, (pt_int[0]-2, pt_int[1]), (pt_int[0]+2, pt_int[1]), color1)
cv2.line(image_viz, (pt_int[0], pt_int[1]-2), (pt_int[0], pt_int[1]+2), color1)
pt_int = tuple([int(round(p)) for p in point2Ds_p_nd[i][0]])
cv2.line(image_viz, (pt_int[0]-2, pt_int[1]), (pt_int[0]+2, pt_int[1]), color3)
cv2.line(image_viz, (pt_int[0], pt_int[1]-2), (pt_int[0], pt_int[1]+2), color3)
pt_int = tuple([int(round(p)) for p in point2Ds_p[i][0]])
def draw_quads(self, img, quads, color=(0, 255, 0)):
img_quads = cv.projectPoints(quads.reshape(-1, 3), self.rvec,
self.tvec, self.K, self.dist_coef)[0]
img_quads.shape = quads.shape[:2] + (2, )
for q in img_quads:
cv.fillConvexPoly(img, np.int32(q * 4), color, cv.LINE_AA, shift=2)
def camera_intrinsic_calibration(req, image_data):
# termination criteria
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
criteria_cal = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 10,
0.001)
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((6 * 7, 3), np.float32)
objp[:, :2] = np.mgrid[0:7, 0:6].T.reshape(-1, 2)
for img in image_data:
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# Find the chess board corners
ret, corners = cv2.findChessboardCorners(gray, (7, 6), None)
# If found, add object points, image points (after refining them)
if ret == True:
objpoints.append(objp)
# Once we find the corners, we can increase their accuracy
corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1),
criteria)
imgpoints.append(corners2)
# index list for imgpoints
list1 = np.arange(0, len(imgpoints), 1)
# camera intrinsic matrix
mtx = np.zeros((3, 3))
if len(req.params) > 3:
mtx[0, 0] = req.params[0]
mtx[1, 1] = req.params[1]
mtx[0, 2] = req.params[2]
mtx[1, 2] = req.params[3]
mtx[2, 2] = 1
# optimize data step by step based on sampled imgs, get best one
min_error = 1000
best_mtx = mtx
for i in range(10):
cur_data = list1
if len(imgpoints) > 20:
# randomly select 20 keypoints to do calibration
cur_data = sample(list1, 20)
cur_obj = list(objpoints[i] for i in cur_data)
cur_img = list(imgpoints[i] for i in cur_data)
try:
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
cur_obj,
cur_img,
gray.shape[::-1],
cameraMatrix=mtx,
distCoeffs=None,
rvecs=None,
tvecs=None,
flags=0,
criteria=criteria_cal)
except:
gray = cv2.cvtColor(image_data[0], cv2.COLOR_BGR2GRAY)
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
cur_obj,
cur_img,
gray.shape[::-1],
cameraMatrix=mtx,
distCoeffs=None,
rvecs=None,
tvecs=None,
flags=0,
criteria=criteria_cal)
#evaluate
tot_error = 0
mean_error = 0
for j in range(len(cur_obj)):
imgpoints2, _ = cv2.projectPoints(cur_obj[j], rvecs[j], tvecs[j],
mtx, dist)
error = cv2.norm(cur_img[j], imgpoints2,
cv2.NORM_L2) / len(imgpoints2)
tot_error += error
mean_error = tot_error / len(cur_obj)
if mean_error < min_error:
min_error = mean_error
best_mtx = mtx
rospy.loginfo(rospy.get_caller_id() + 'I get corners %s',
len(imgpoints))
rospy.loginfo(rospy.get_caller_id() + 'I get parameters %s',
best_mtx[0, 0])
return imgpoints, best_mtx
for i, j in zip(range(4), range(4, 8)):
img = cv.line(img, tuple(imgpts[i]), tuple(imgpts[j]), (255), 3)
# draw top layer in red color
img = cv.drawContours(img, [imgpts[4:]], -1, (0, 0, 255), 3)
return img
criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001)
objp = np.zeros((6 * 7, 3), np.float32)
objp[:, :2] = np.mgrid[0:7, 0:6].T.reshape(-1, 2)
#axis = np.float32([[3,0,0], [0,3,0], [0,0,-3]]).reshape(-1,3)
axis = np.float32([[0, 0, 0], [0, 3, 0], [3, 3, 0], [3, 0, 0], [0, 0, -3],
[0, 3, -3], [3, 3, -3], [3, 0, -3]])
for fname in glob.glob('images/*.jpg'):
img = cv.imread(fname)
img = cv.resize(img, None, fx=0.4, fy=0.4)
gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
ret, corners = cv.findChessboardCorners(gray, (7, 6), None)
if ret == True:
corners2 = cv.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
# Find the rotation and translation vectors.
ret, rvecs, tvecs, _ = cv.solvePnPRansac(objp, corners2, K, d)
# project 3D points to image plane
imgpts, jac = cv.projectPoints(axis, rvecs, tvecs, K, d)
img = draw(img, corners2, imgpts)
cv.imshow('img', img)
k = cv.waitKey(1000)
if k == ord('s'):
cv.imwrite(fname[:6] + '.png', img)
cv.destroyAllWindows()
def project_many(self, points):
"""Project 3D points in camera coordinates to the image plane."""
distortion = np.array([self.k1, self.k2, self.p1, self.p2, self.k3])
K, R, t = self.get_K(), np.zeros(3), np.zeros(3)
pixels, _ = cv2.projectPoints(points, R, t, K, distortion)
return pixels.reshape((-1, 2))
def rms_calc(objpoints, rvecs, tvecs, mtx, dist):
mean_error = 0
imgpoints2, jacob = cv2.projectPoints(objpoints[0], rvecs, tvecs, mtx,
dist)
return imgpoints2
def direct(fiducialPoints, rVec, tVec, linearCoeffs, distCoeffs):
projected, _ = projectPoints(fiducialPoints, rVec, tVec, linearCoeffs,
distCoeffs)
return projected
def Stereo_Paras(self):
# criteria_stereo = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
# Detect the patterns from the calibration board
objpoints, imgpointsL, imgpointsR, ChessImaL, ChessImaR = self.Stereo_Calib(
)
# Get some images for the size of the image
gray_left = cv2.cvtColor(ChessImaL, cv2.COLOR_BGR2GRAY)
gray_right = cv2.cvtColor(ChessImaR, cv2.COLOR_BGR2GRAY)
# Determine the new values for different parameters
# Calibration_Right_Logitech camera calibration
retR, mtxR, distR, rvecsR, tvecsR = cv2.calibrateCamera(
objpoints, imgpointsR, gray_right.shape[::-1], None, None)
hR, wR = ChessImaR.shape[:2] # The size of the image
OmtxR, roiR = cv2.getOptimalNewCameraMatrix(mtxR, distR, (wR, hR), 1,
(wR, hR))
# ROI used to crop the image
# Calibration_Left_Logitech camera calibration
retL, mtxL, distL, rvecsL, tvecsL = cv2.calibrateCamera(
objpoints, imgpointsL, gray_left.shape[::-1], None, None)
hL, wL = ChessImaL.shape[:2]
OmtxL, roiL = cv2.getOptimalNewCameraMatrix(mtxL, distL, (wL, hL), 1,
(wL, hL))
# Check the reprojection errors
mean_error_L = 0
tot_error_L = 0
mean_error_R = 0
tot_error_R = 0
for i in range(len(objpoints)):
imgpoints2_L, _ = cv2.projectPoints(objpoints[i], rvecsL[i],
tvecsL[i], mtxL, distL)
error = cv2.norm(imgpointsL[i], imgpoints2_L,
cv2.NORM_L2) / len(imgpoints2_L)
tot_error_L += error
for i in range(len(objpoints)):
imgpoints2_R, _ = cv2.projectPoints(objpoints[i], rvecsR[i],
tvecsR[i], mtxR, distR)
error = cv2.norm(imgpointsR[i], imgpoints2_R,
cv2.NORM_L2) / len(imgpoints2_R)
tot_error_R += error
print("Calibration_Left_Logitech total error: ", tot_error_L)
print("The total number of left points ", len(objpoints))
print("Calibration_Left_Logitech mean error: ",
tot_error_L / len(objpoints))
print("Calibration_Right_Logitech total error: ", tot_error_R)
print("The total number of right points ", len(objpoints))
print("Calibration_Right_Logitech mean error: ",
tot_error_R / len(objpoints))
# Stereo calibrate function
flags = 0
flags |= cv2.CALIB_FIX_INTRINSIC
retS = None
MLS = None
dLS = None
dRS = None
R = None
T = None
E = None
F = None
retS, MLS, dLS, MRS, dRS, R, T, E, F = cv2.stereoCalibrate(
objpoints,
imgpointsL,
imgpointsR,
mtxL,
distL,
mtxR,
distR,
gray_right.shape[::-1],
flags=cv2.CALIB_FIX_INTRINSIC)
# criteria_stereo,
# flags)
print("The translation between the first and second camera is ", T)
'''
Stereo Rectification Process
'''
# StereoRectify function
# last paramater is alpha, if 0= croped, if 1= not croped
rectify_scale = 0 # if 0 image croped, if 1 image nor croped
RL, RR, PL, PR, Q, roiL, roiR = cv2.stereoRectify(
MLS, dLS, MRS, dRS, gray_right.shape[::-1], R, T, rectify_scale,
(0, 0))
# print("The Q matrix is", Q)
# initUndistortRectifyMap function -- map the images to the undistorted images
# cv2.CV_16SC2 this format enables us the programme to work faster
Left_Stereo_Map = cv2.initUndistortRectifyMap(MLS, dLS, RL, PL,
gray_left.shape[::-1],
cv2.CV_16SC2)
Right_Stereo_Map = cv2.initUndistortRectifyMap(MRS, dRS, RR, PR,
gray_right.shape[::-1],
cv2.CV_16SC2)
return Left_Stereo_Map, Right_Stereo_Map, Q
def chessboard_pose(img_dir,
img_filename,
cam_mtx,
cam_dist,
objp,
pattern=(7, 6)):
"""
Find the chessboard pose with OpenCV.
@type img_dir: string
@param img_dir: directory of the image
@type img_filename: string
@param img_filename: filename of the image
@type cam_mtx: numpy.ndarray
@param cam_mtx: intrinsic matrix of the camera
@type cam_dist: numpy.ndarry
@param cam_dist: distortion coefficient of the camera
@type objp: numpy.ndarray
@param objp: 3D positions of the points on the chessboard
@type pattern: tuple
@param pattern: pattern of the chessboard
"""
img_filepath = os.path.join(img_dir, img_filename)
img_name = img_filename.split('.')[0]
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100,
0.0005)
chessboard_size_tuple = pattern
# IR and RGB both read as RGB and then turned to gray
img = cv2.imread(img_filepath)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, chessboard_size_tuple, None)
if ret == True:
# Increase corner accuracy
corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1),
criteria)
cv2.drawChessboardCorners(img, chessboard_size_tuple, corners, ret)
cv2.imwrite(os.path.join(img_dir, img_name + "_corner.png"), img)
# print ("Camera Info")
# print (cam_mtx)
# print (cam_dist)
_, rvecs, tvecs, inlier = cv2.solvePnPRansac(objp, corners2, cam_mtx,
cam_dist)
# print ("aa")
# print (rvecs)
# print ("t")
# print (tvecs)
R, _ = cv2.Rodrigues(rvecs)
# print ("rotm")
# print (R)
imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, cam_mtx, cam_dist)
imgpts = np.int32(imgpts).reshape(-1, 2)
# print ("imgpts")
# print (imgpts)
img = draw(img, imgpts)
cv2.imwrite(os.path.join(img_dir, img_name + '_axis.png'), img)
print("Finish processing: {}".format(img_filename))
return R, tvecs
else:
print("Cannot find chessboard in {}".format(img_filename))
return None, None
def main():
tracker = FaceTracker()
capturing = False
captured_eye_data = []
captured_head_data = []
captured_mouse_data = []
while True:
img = np.ones((1920, 1080, 3), np.uint8)
cv2.imshow('trainer', img)
cv2.setWindowProperty('trainer', cv2.WND_PROP_FULLSCREEN,
cv2.WINDOW_FULLSCREEN)
cv2.moveWindow('trainer', 0, 0)
frame = tracker.tick()
if DEBUG:
text_x1 = tracker.face_x1
text_y1 = tracker.face_y1 - 3
if text_y1 < 0: text_y1 = 0
cv2.putText(frame, "FACE", (text_x1, text_y1),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1)
cv2.rectangle(frame, (tracker.face_x1, tracker.face_y1),
(tracker.face_x2, tracker.face_y2), (0, 255, 0), 2)
if tracker.face_type > 0:
if DEBUG:
for point in tracker.landmarks_2D:
cv2.circle(frame, (point[0], point[1]), 2, (0, 0, 255), -1)
(mx, my) = mouse_pos()
axis = np.float32([[50, 0, 0], [0, 50, 0], [0, 0, 50]])
imgpts, jac = cv2.projectPoints(axis, tracker.rvec, tracker.tvec,
tracker.camera_matrix,
tracker.camera_distortion)
sellion_xy = (tracker.landmarks_2D[7][0],
tracker.landmarks_2D[7][1])
cv2.line(frame, sellion_xy, tuple(imgpts[1].ravel()), (0, 255, 0),
3) #GREEN
cv2.line(frame, sellion_xy, tuple(imgpts[2].ravel()), (255, 0, 0),
3) #BLUE
cv2.line(frame, sellion_xy, tuple(imgpts[0].ravel()), (0, 0, 255),
3) #RED
if tracker.left_eye is not None and tracker.right_eye is not None:
cv2.imshow('left eye', tracker.left_eye)
cv2.imshow('right eye', tracker.right_eye)
if capturing:
captured_eye_data.append(
(tracker.left_eye, tracker.right_eye))
captured_head_data.append((tracker.rvec, tracker.tvec))
captured_mouse_data.append((mx, my))
cv2.circle(frame, (10, 10), 5, (0, 0, 255), -1)
if DEBUG:
text_x1 = tracker.roi_x1
text_y1 = tracker.roi_y1 - 3
if text_y1 < 0: text_y1 = 0
cv2.putText(frame, "ROI", (text_x1, text_y1),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 255), 1)
cv2.rectangle(frame, (tracker.roi_x1, tracker.roi_y1),
(tracker.roi_x2, tracker.roi_y2), (0, 255, 255), 2)
cv2.imshow('Video', frame)
key = cv2.waitKey(1) & 0xFF
capturing = key == ord('c')
if key == ord('w'):
num_existing_pairs = len(os.listdir('./data/imgs/')) / 2
for idx, (left, right) in enumerate(captured_eye_data):
(left_gauss, right_gauss) = add_gaussian_noise([left, right])
cv2.imwrite(
'data/imgs/l/' +
str(int((idx * 2 + 1) + num_existing_pairs)) + '.png',
left_gauss)
cv2.imwrite(
'data/imgs/r/' +
str(int((idx * 2 + 1) + num_existing_pairs)) + '.png',
right_gauss)
cv2.imwrite(
'data/imgs/l/' + str(int((idx * 2) + num_existing_pairs)) +
'.png', left)
cv2.imwrite(
'data/imgs/r/' + str(int((idx * 2) + num_existing_pairs)) +
'.png', right)
with open('data/data.csv', 'a') as file:
for idx, (mx, my) in enumerate(captured_mouse_data):
(rvec, tvec) = captured_head_data[idx]
file.write(
str(rvec[0][0]) + ',' + str(rvec[1][0]) + ',' +
str(rvec[2][0]) + ',' + str(tvec[0][0]) + ',' +
str(tvec[1][0]) + ',' + str(tvec[2][0]) + ',' +
str(mx) + ',' + str(my))
file.write('\n')
file.write(
str(rvec[0][0]) + ',' + str(rvec[1][0]) + ',' +
str(rvec[2][0]) + ',' + str(tvec[0][0]) + ',' +
str(tvec[1][0]) + ',' + str(tvec[2][0]) + ',' +
str(mx) + ',' +
str(my)) # write twice due to gauss augmentation
file.write('\n')
print("Wrote " + str(len(captured_eye_data) * 2) +
" data points to disk.")
captured_eye_data.clear()
captured_mouse_data.clear()
captured_head_data.clear()
if key == ord('q'): break
def camera_lidar_read(self, image_pub=None):
global CAMERA_MODEL, FIRST_TIME, PAUSE, TF_BUFFER, TF_LISTENER, CAMERA_MODEL_INITED, PROJECT_MODE
# # Projection/display mode
# if CAMERA_MODEL_INITED and PROJECT_MODE:
# rospy.loginfo("Entering project_point_cloud")
# self.project_point_cloud(pointcloud_msg, image_msg, image_pub)
# PROJECT_MODE = False
# Calibration mode
# elif PAUSE:
i = 1
while (i < len(self.pcd_files) + 1):
# image_filename = self.image_files[i]
image_filename = os.path.join(self.path, self.image_filename % i)
# pcd_filename = self.pcd_files[i]
pcd_filename = os.path.join(self.path, self.pcd_filename % i)
rospy.loginfo("Frame %d. image file: %s. pcd file: %s" %
(i, os.path.basename(image_filename),
os.path.basename(pcd_filename)))
if CAMERA_MODEL_INITED and PROJECT_MODE:
rospy.loginfo("Entering project_point_cloud")
self.project_point_cloud(pcd_filename, image_filename,
image_pub)
PROJECT_MODE = False
self.board_points_num = len(self.board_points)
self.pixel_points_num = len(self.pixel_points)
self.lidar_points_num = len(self.lidar_points)
# Create GUI processes
now = rospy.get_rostime()
# proc_pool = multiprocessing.Pool(processes=2)
# proc_results = []
self.proc_results = multiprocessing.Queue()
# proc_results.append(proc_pool.apply_async(self.extract_points_3D, args=(pointcloud_msg, self.lidar_points,)))
pcl_proc = multiprocessing.Process(
target=self.extract_points_3D,
args=[pcd_filename, self.proc_results])
self.procs.append(pcl_proc)
# proc_results.append(proc_pool.apply_async(self.extract_points_2D, args=(image_msg, self.board_points, self.pixel_points,)))
img_proc = multiprocessing.Process(
target=self.extract_points_2D,
args=[image_filename, self.proc_results])
self.procs.append(img_proc)
# pool.close() # 关闭进程池，不再接受新的进程
# pool.join() # 主进程阻塞等待子进程的退出
# rospy.loginfo("Starting sub threads.")
img_proc.start()
pcl_proc.start()
img_proc.join()
pcl_proc.join()
# rospy.loginfo("All sub threads ended.")
results = []
results_valid = True
if (self.proc_results.empty()):
results_valid = False
else:
try:
for proc in self.procs:
results.append(self.proc_results.get_nowait())
except Exception:
rospy.logwarn("Get results error. Pass this frame.")
results_valid = False
if (results_valid and len(results) > 1):
if (len(results[0]) == 1 and len(results[1]) == 2):
self.lidar_points.append(results[0][0])
self.board_points.append(results[1][0])
self.pixel_points.append(results[1][1])
elif (len(results[1]) == 1 and len(results[0]) == 2):
self.lidar_points.append(results[1][0])
self.board_points.append(results[0][0])
self.pixel_points.append(results[0][1])
self.frame_count += 1
print(
"current number of middle varaibles: board_points %d, pixel_points %d, lidar_points %d"
% (len(self.board_points), len(
self.pixel_points), len(self.lidar_points)))
del self.procs
self.procs = []
# Calibrate for existing corresponding points
if (self.frame_count >= self.calibrate_min_frames
and CAMERA_MODEL_INITED
and (self.board_points_num < len(self.board_points))):
ret, self.camera_matrix, self.dist_coeffs, self.chessboard_to_camera_rvecs, self.chessboard_to_camera_tvecs = cv2.calibrateCamera(
self.board_points, self.pixel_points, self.image_shape,
None, None)
# 重投影误差
total_error = 0
for i in range(len(self.board_points)):
imgpoints2, _ = cv2.projectPoints(
self.board_points[i],
self.chessboard_to_camera_rvecs[i],
self.chessboard_to_camera_tvecs[i], self.camera_matrix,
self.dist_coeffs)
error = cv2.norm(np.array(self.pixel_points[i]),
np.squeeze(imgpoints2),
cv2.NORM_L2) / len(imgpoints2)
total_error += error
print("reprojection error for current camera calibration: ",
total_error / len(self.board_points))
print("self.dist_coeffs:", self.dist_coeffs)
# save current camera calibration to file
self.R = np.eye(3, dtype=np.float64)
self.P = np.zeros((3, 4), dtype=np.float64)
ncm, _ = cv2.getOptimalNewCameraMatrix(self.camera_matrix,
self.dist_coeffs,
self.image_shape, 0)
for j in range(3):
for i in range(3):
self.P[j, i] = ncm[j, i]
calibration_string = cameraCalibrationYamlBuf(
self.dist_coeffs,
self.camera_matrix,
self.R,
self.P,
self.image_shape,
name=self.camera_name)
if (not os.path.exists(os.path.join(PKG_PATH, CALIB_PATH))):
os.mkdir(os.path.join(PKG_PATH, CALIB_PATH))
with open(
os.path.join(
PKG_PATH, CALIB_PATH,
"intrinsics_" + self.camera_name + ".yaml"),
'a') as f:
f.write(calibration_string)
CAMERA_MODEL_INITED = True
CAMERA_MODEL.K = mkmat(3, 3, self.camera_matrix)
if (self.dist_coeffs is not None):
CAMERA_MODEL.D = mkmat(len(self.dist_coeffs.squeeze()), 1,
self.dist_coeffs)
else:
CAMERA_MODEL.D = None
CAMERA_MODEL.R = mkmat(3, 3, self.R)
CAMERA_MODEL.P = mkmat(3, 4, self.P)
CAMERA_MODEL.full_K = mkmat(3, 3, self.camera_matrix)
CAMERA_MODEL.full_P = mkmat(3, 4, self.P)
CAMERA_MODEL.width = self.image_shape[1]
CAMERA_MODEL.height = self.image_shape[0]
CAMERA_MODEL.resolution = (CAMERA_MODEL.width,
CAMERA_MODEL.height)
CAMERA_MODEL.tf_frame = self.camera_name
self.chessboard_to_camera_R = [
cv2.Rodrigues(x)[0]
for x in self.chessboard_to_camera_rvecs
]
rospy.loginfo("Entering calibration")
self.calibrate()
if (self.success):
self.calibration_finished = True
PROJECT_MODE = True
key = six.moves.input(
'Press [ENTER] to continue or [q] to quit or [r] to process current frame again: '
)
if (key == 'q'):
if (key == 'q'):
print("Exit.")
sys.exit()
elif (key != 'r'):
i += 1
print("[ENTER] pressed. Process next frame.")
else:
print("Process frame %d again" % i)
def calibrate(self, video_source="./", dim_x=7, dim_y=7):
"""
This will call a reference to a video taken, based on ?camera information?
and will calibrate the camera and save the results in self.calibrated
Probably calibrate to default video link
With the calibration we get the distortion coefficients.
NOTE We use a 7x7 grid
Returns
-------
Camera matrix, distortion coefficients, rotation and translation vectors
Distortion Coefficients = (k1, k2, p1, p2, k3)
Camera Matrix:
focal length (fx,fy), optical centers (cx, cy)
[fx, 0 , cx]
[0 , fy, cy]
[0 , 0 , 1 ]
"""
cap = cv2.VideoCapture(video_source)
if not cap.isOpened():
raise ValueError("Unable to open video source", video_source)
print("Calibrating Camera...")
cap = cv2.VideoCapture(video_source)
while True:
corners_found = 0
# termination criteria
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((dim_x*dim_y, 3), np.float32)
objp[:, :2] = np.mgrid[0:dim_x, 0:dim_y].T.reshape(-1, 2)
# Arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
framepoints = [] # 2d points in image plane.
ret, frame = cap.read()
if ret:
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
if cv2.waitKey(30) & 0xFF == ord('q'):
break
# print("Finding Corners... ", end ="")
# Find the chess board corners
ret, corners = cv2.findChessboardCorners(gray, (dim_x, dim_y), None)
# print(ret)
# If found, add object points, image points (after refining them)
if ret:
corners_found += 1
objpoints.append(objp)
corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
framepoints.append(corners2)
# Draw and display the corners
frame = cv2.drawChessboardCorners(frame, (dim_x, dim_y), corners2, ret)
cv2.imshow('frame', frame)
ret, cam_mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, framepoints, gray.shape[::-1], None,None)
w = np.size(frame, 1)
h = np.size(frame, 0)
newcameramtx, roi=cv2.getOptimalNewCameraMatrix(cam_mtx, dist, (w, h), 1, (w, h))
# undistort
dst = cv2.undistort(frame, cam_mtx, dist, None, newcameramtx)
# crop the image
x,y,w,h = roi
dst = dst[y:y+h, x:x+w]
if x+w == 0:
print(cam_mtx)
print("Bad export. Trying next frame")
continue
# crop the image
x,y,w,h = roi
dst = dst[y:y+h, x:x+w]
cv2.imwrite('calibresult.png',dst)
mean_error = 0
tot_error = 0
for i in range(len(objpoints)):
framepoints2, _ = cv2.projectPoints(objpoints[i], rvecs[i], tvecs[i], cam_mtx, dist)
error = cv2.norm(framepoints[i], framepoints2, cv2.NORM_L2)/len(framepoints2)
tot_error += error
print("Mean Error: " + str(mean_error/len(objpoints)))
#we found a valid frame so we can use it to calculate
break
else:
print("Cannot find corners")
else:
break
cap.release()
cv2.destroyAllWindows()
return cam_mtx, newcameramtx
(R, T) = util.camera_pose_from_homography(mtx_inv, H)
rvec_, _ = cv2.Rodrigues(R.T)
r = rot.from_rotvec(rvec_.T).as_euler('xyz', degrees=True)
cv2.putText(
img,
'Rotation(Euler angles): X: {:0.2f} Y: {:0.2f} Z: {:0.2f}'.
format(r[0][0], r[0][1],
r[0][2]), (20, int(frame_height) - 20),
cv2.FONT_HERSHEY_SIMPLEX, .5, (255, 255, 255))
cv2.putText(
img,
'Translation(mm): X: {:0.2f} Y: {:0.2f} Z: {:0.2f}'.format(
T[0], T[1], T[2]), (20, int(frame_height) - 60),
cv2.FONT_HERSHEY_SIMPLEX, .5, (255, 255, 255))
# Project the boundary of the planar target to the image.
pts = np.float32([[0, 0, 0], [0, PHY_HEIGHT - 1, 0],
[PHY_WIDTH - 1, PHY_HEIGHT - 1, 0],
[PHY_WIDTH - 1, 0, 0]]).reshape(-1, 1, 3)
dst, jac = cv2.projectPoints(pts, rvec_, T, mtx, dist)
img = cv2.polylines(img, [np.int32(dst)], True,
(100, 230, 240), 3, cv2.LINE_AA)
# Project 3D axes points to the image.
img_axes_pts, jac = cv2.projectPoints(axes, rvec_, T, mtx,
dist)
img = util.draw_axes(img, np.int32(img_axes_pts))
cv2.imshow('img', img)
key = cv2.waitKey(delay=1) & 0xFF
if key == ord("q"):
break
cv2.destroyAllWindows()
def image_jacobian_gen(self, result, corners, ids, CamParam, board_ground,board_panel,id_start_ground, id_board_ground_size, tag_ground_size, loaded_object_points_ground_in_panel_system, display_window):
rospy.loginfo(str(id_start_ground))
rospy.loginfo(str(id_board_ground_size))
rospy.loginfo(str(tag_ground_size))
rospy.loginfo(str(corners)) #float32
rospy.loginfo(str(board_ground))
rospy.loginfo(str(board_panel))
idx_max = 180
UV = np.zeros([idx_max,8])
P = np.zeros([idx_max,3])
id_valid = np.zeros([idx_max,1])
f_hat_u = CamParam.camMatrix[0][0]
f_hat_v = CamParam.camMatrix[1][1]
f_0_u = CamParam.camMatrix[0][2]
f_0_v = CamParam.camMatrix[1][2]
imgPoints_ground, rvec_ground, tvec_ground, Rca_ground, b_ground = self.get_pose(board_ground, corners, ids, CamParam)
imgPoints_panel, rvec_panel, tvec_panel, Rca_panel, b_panel = self.get_pose(board_panel, corners, ids, CamParam)
corners_ground = []
corners_panel = []
for i_ids,i_corners in zip(ids,corners):
if i_ids<=(id_start_ground+id_board_ground_size):
corners_ground.append(i_corners)
else:
corners_panel.append(i_corners)
#rospy.loginfo(str(id_start_ground))
rvec_all_markers_ground, tvec_all_markers_ground, _ = aruco.estimatePoseSingleMarkers(corners_ground, tag_ground_size, CamParam.camMatrix, CamParam.distCoeff)
rvec_all_markers_panel, tvec_all_markers_panel, _ = aruco.estimatePoseSingleMarkers(corners_panel, 0.025, CamParam.camMatrix, CamParam.distCoeff)
#rospy.loginfo(str(tvec_all_markers_ground))
#rospy.loginfo(str(tvec_all_markers_panel))
tvec_all=np.concatenate((tvec_all_markers_ground,tvec_all_markers_panel),axis=0)
for i_ids,i_corners,i_tvec in zip(ids,corners,tvec_all):
if i_ids<idx_max:
#print 'i_corners',i_corners,i_corners.reshape([1,8])
UV[i_ids,:] = i_corners.reshape([1,8]) #np.average(i_corners, axis=1)
P[i_ids,:] = i_tvec
id_valid[i_ids] = 1
id_select = range(id_start_ground,(id_start_ground+id_board_ground_size))
#used to find the height of the tags and the delta change of height, z height at desired position
Z = P[id_select,2] #- [0.68184539, 0.68560932, 0.68966803, 0.69619578])
id_valid = id_valid[id_select]
dutmp = []
dvtmp = []
#Pixel estimates of the ideal ground tag location
reprojected_imagePoints_ground_2, jacobian2 = cv2.projectPoints( loaded_object_points_ground_in_panel_system.transpose(), rvec_panel, tvec_panel, CamParam.camMatrix, CamParam.distCoeff)
reprojected_imagePoints_ground_2 = reprojected_imagePoints_ground_2.reshape([reprojected_imagePoints_ground_2.shape[0],2])
if(display_window):
frame_with_markers_and_axis = cv2.cvtColor(result, cv2.COLOR_GRAY2BGR)
frame_with_markers_and_axis = cv2.aruco.drawAxis( frame_with_markers_and_axis, CamParam.camMatrix, CamParam.distCoeff, Rca_ground, tvec_ground, 0.2 )
frame_with_markers_and_axis = cv2.aruco.drawAxis( frame_with_markers_and_axis, CamParam.camMatrix, CamParam.distCoeff, Rca_panel, tvec_panel, 0.2 )
#plot image points for ground tag from corner detection and from re-projections
for point1,point2 in zip(imgPoints_ground,np.float32(reprojected_imagePoints_ground_2)):
cv2.circle(frame_with_markers_and_axis,tuple(point1),5,(0,0,255),3)
cv2.circle(frame_with_markers_and_axis,tuple(point2),5,(255,0,0),3)
height, width, channels = frame_with_markers_and_axis.shape
cv2.imshow(display_window,cv2.resize(frame_with_markers_and_axis, (width/4, height/4)))
cv2.waitKey(1)
#Save
#filename_image = "/home/rpi-cats/Desktop/DJ/Code/Images/Panel2_Acquisition_"+str(t1)+"_"+str(iteration)+".jpg"
#scipy.misc.imsave(filename_image, frame_with_markers_and_axis)
reprojected_imagePoints_ground_2 = np.reshape(reprojected_imagePoints_ground_2,(id_board_ground_size,8))
#Go through a particular point in all tags to build the complete Jacobian
for ic in range(4):
#uses first set of tags, numbers used to offset camera frame, come from camera parameters
UV_target = np.vstack([reprojected_imagePoints_ground_2[:,2*ic]-f_0_u, reprojected_imagePoints_ground_2[:,2*ic+1]-f_0_v]).T
uc = UV_target[:,0]
vc = UV_target[:,1]
UV_current = np.vstack([UV[id_select,2*ic]-f_0_u, UV[id_select,2*ic+1]-f_0_v]).T
#find difference between current and desired tag difference
delta_UV = UV_target-UV_current
delet_idx = []
J_cam_tmp =np.array([])
for tag_i in range(id_board_ground_size):
if id_valid[tag_i] == 1:
tmp = 1.0*np.array([[uc[tag_i]*vc[tag_i]/f_hat_u, -1.0*(uc[tag_i]*uc[tag_i]/f_hat_u + f_hat_u), vc[tag_i],-f_hat_u/Z[tag_i], 0.0, uc[tag_i]/Z[tag_i]],
[ vc[tag_i]*vc[tag_i]/f_hat_v+f_hat_v, -1.0*uc[tag_i]*vc[tag_i]/f_hat_v, -uc[tag_i],0.0, -f_hat_v/Z[tag_i], vc[tag_i]/Z[tag_i]]])
if not (J_cam_tmp).any():
J_cam_tmp = tmp
else:
J_cam_tmp= np.concatenate((J_cam_tmp, tmp), axis=0)
else:
delet_idx.append(tag_i)
delta_UV = np.delete(delta_UV, delet_idx, 0)
dutmp.append(np.mean(delta_UV[:,0]))
dvtmp.append(np.mean(delta_UV[:,1]))
#camera jacobian
if ic ==0:
J_cam = J_cam_tmp
delta_UV_all = delta_UV.reshape(np.shape(delta_UV)[0]*np.shape(delta_UV)[1],1)
UV_target_all = UV_target.reshape(np.shape(UV_target)[0]*np.shape(UV_target)[1],1)
else:
J_cam = np.vstack([J_cam,J_cam_tmp])
delta_UV_all = np.vstack([delta_UV_all,delta_UV.reshape(np.shape(delta_UV)[0]*np.shape(delta_UV)[1],1)])
UV_target_all = np.vstack([UV_target_all,UV_target.reshape(np.shape(UV_target)[0]*np.shape(UV_target)[1],1)])
du = np.mean(np.absolute(dutmp))
dv = np.mean(np.absolute(dvtmp))
print 'Average du of all points:',du
print 'Average dv of all points:',dv
return du, dv, J_cam, delta_UV_all
None)
img = cv.imread('/home/farman/Downloads/object1.jpg')
h, w = img.shape[:2]
newcameramtx, roi = cv.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1,
(w, h))
mapx, mapy = cv.initUndistortRectifyMap(mtx, dist, None, newcameramtx,
(w, h), 5)
dst = cv.remap(img, mapx, mapy, cv.INTER_LINEAR)
# crop the image
x, y, w, h = roi
print(roi)
dst = dst[y:y + h, x:x + w]
cv.imwrite('calibresult.png', dst)
print(ret)
print('calibration matrix')
print(mtx)
print('distortion coefficients')
print(dist)
total_error = 0
for i in range(len(objpoints)):
imgpoints2, _ = cv.projectPoints(objpoints[i], rvecs[i], tvecs[i], mtx,
dist)
error = cv.norm(imgpoints[i], imgpoints2, cv.NORM_L2) / len(imgpoints2)
total_error += error
print("mean error: {}".format(total_error / len(objpoints)))
print('rotational verstors:', rvecs)
print('???????????????????????????????????????????????')
print('translational vectors', tvecs)
dist_coeffs = np.zeros((4, 1)) # Assuming no lens distortion
(success, rotation_vector, translation_vector) = cv2.solvePnP(
model_points,
image_points,
camera_matrix,
dist_coeffs,
flags=cv2.SOLVEPNP_ITERATIVE) #flags=cv2.CV_ITERATIVE)
print "Rotation Vector:\n {0}".format(rotation_vector)
print "Translation Vector:\n {0}".format(translation_vector)
# Project a 3D point (0, 0 , 1000.0) onto the image plane
# We use this to draw a line sticking out of the nose_end_point2D
(nose_end_point2D,
jacobian) = cv2.projectPoints(np.array([(0.0, 0.0, 1000.0)]),
rotation_vector, translation_vector,
camera_matrix, dist_coeffs)
for p in image_points:
cv2.circle(frame, (int(p[0]), int(p[1])), 3, (0, 0, 255), -1)
p1 = (int(image_points[0][0]), int(image_points[0][1]))
p2 = (int(nose_end_point2D[0][0][0]), int(nose_end_point2D[0][0][1]))
cv2.line(frame, p1, p2, (255, 0, 0), 2)
# show the frame
cv2.imshow("Frame", frame)
key = cv2.waitKey(1) & 0xFF
# if the `q` key was pressed, break from the loop
if key == ord("q"):
def prod_trans_point(p3d, rotation_vector, trans_vector, camera_matrix,
dist_coeffs):
plane_point, _ = cv2.projectPoints(np.array([p3d]), rotation_vector,
trans_vector, camera_matrix,
dist_coeffs)
return (int(plane_point[0][0][0]), int(plane_point[0][0][1]))
def _map_binocular(self, p0, p1):
if '3d' not in p0['method'] or '3d' not in p1['method']:
return None
#find the nearest intersection point of the two gaze lines
# a line is defined by two point
s0_center = self.eye0_to_World(np.array(p0['sphere']['center']))
s1_center = self.eye1_to_World(np.array(p1['sphere']['center']))
s0_normal = np.dot(self.rotation_matricies[0],
np.array(p0['circle_3d']['normal']))
s1_normal = np.dot(self.rotation_matricies[1],
np.array(p1['circle_3d']['normal']))
gaze_line0 = [s0_center, s0_center + s0_normal]
gaze_line1 = [s1_center, s1_center + s1_normal]
nearest_intersection_point, intersection_distance = math_helper.nearest_intersection(
gaze_line0, gaze_line1)
if nearest_intersection_point is not None:
self.last_gaze_distance = np.sqrt(
nearest_intersection_point.dot(nearest_intersection_point))
image_point, _ = cv2.projectPoints(
np.array([nearest_intersection_point]),
np.array([0.0, 0.0, 0.0]), np.array([0.0, 0.0, 0.0]),
self.camera_matrix, self.dist_coefs)
image_point = image_point.reshape(-1, 2)
image_point = normalize(image_point[0],
self.world_frame_size,
flip_y=True)
if self.visualizer.window:
gaze0_3d = s0_normal * self.last_gaze_distance + s0_center
gaze1_3d = s1_normal * self.last_gaze_distance + s1_center
self.gaze_pts_debug0.append(gaze0_3d)
self.gaze_pts_debug1.append(gaze1_3d)
if nearest_intersection_point is not None:
self.intersection_points_debug.append(
nearest_intersection_point)
self.sphere0['center'] = s0_center #eye camera coordinates
self.sphere0['radius'] = p0['sphere']['radius']
self.sphere1['center'] = s1_center #eye camera coordinates
self.sphere1['radius'] = p1['sphere']['radius']
if nearest_intersection_point is None:
return None
confidence = (p0['confidence'] + p1['confidence']) / 2.
ts = (p0['timestamp'] + p1['timestamp']) / 2.
g = {
'norm_pos': image_point,
'eye_centers_3d': {
0: s0_center.tolist(),
1: s1_center.tolist()
},
'gaze_normals_3d': {
0: s0_normal.tolist(),
1: s1_normal.tolist()
},
'gaze_point_3d': nearest_intersection_point.tolist(),
'confidence': confidence,
'timestamp': ts,
'base': [p0, p1]
}
return g
