def calculate_object_pose_ransac(self, image_raw, camera_matrix, dist_coeffs, is_grayscale):
'''
Calculate 3D pose of calibration object in image given the camera's
camera matrix and distortion coefficients.
Returns rotation matrix and translation vector.
@param image_raw: input image, not distortion corrected
@type image_raw: cv2 compatible numpy image
@param camera_matrix: camera matrix of camera
@type camera_matrix: numpy matrix
@param dist_coeffs: distortion coefficients of camera
@type dist_coeffs: numpy matrix
@param is_grayscale: set to true if image is grayscale
@type is_grayscale: bool
'''
# get image and object points
image_points = self.detect_image_points(image_raw, is_grayscale)
object_points = self.calibration_object.get_pattern_points_center()
# get object pose in raw image (not yet distortion corrected)
#(retval, rvec, tvec) = cv2.solvePnP(object_points, image_points, camera_matrix, dist_coeffs)
(rvec,tvec,inliers)= cv2.solvePnPRansac(object_points, image_points, camera_matrix, dist_coeffs)
# convert rvec to rotation matrix
rmat = cv2.Rodrigues(rvec)[0]
return (image_points[1],rmat, tvec)
def findTransformation(K, corr): objPoints = np.asarray([c[1].x[0:3] for c in corr],dtype=np.float32) imgPoints = np.asarray([c[0] for c in corr],dtype=np.float32) rvec,tvec,inliers = cv2.solvePnPRansac(objPoints,imgPoints,K,None) rot,_ = cv2.Rodrigues(rvec) trans = -tvec[:,0] return createP(K,rot,trans)
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 estimate_pose(self, cam_matrix, dist_mat):
search_size = (5, 4)
axis = np.float32(([[3, 0, 0], [0, 3, 0], [0, 0, 3]])).reshape(-1, 3)
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
objp = np.zeros((search_size[0] * search_size[1], 3), np.float32)
objp[:, :2] = np.mgrid[0:search_size[0], 0:search_size[1]].T.reshape(-1, 2)
cap = cv2.VideoCapture('../videos/right_sd_test2.avi')
while(cap.isOpened()):
ret, frame = cap.read()
grey = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(grey, search_size, None)
if ret:
cv2.cornerSubPix(grey, corners, (11, 11), (-1, -1), criteria)
rvecs, tvecs, inliers = cv2.solvePnPRansac(objp, corners, cam_matrix, dist_mat)
cv2.imshow('frame', frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
cap.release()
cv2.destroyAllWindows()
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 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 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 compute_cam_pose(K_matrices, matched_pts_2D, matched_pts_3D, poses):
'''Compute the camera pose from a set of 3D and 2D correspondences.'''
K1 = K_matrices[-2]
rvec, tvec = cv2.solvePnPRansac(matched_pts_3D, matched_pts_2D, K1, None)[0:2]
rmat = cv2.Rodrigues(rvec)[0]
pose = np.hstack((rmat, tvec))
poses.append(pose)
return poses
def main():
data = np.load('cameraParams.npz')
mtx = data['intrinsic']
dist = data['distortion']
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.00001)
objp = np.zeros((gridW*gridH,3), np.float32)
objp[:,:2] = np.mgrid[0:gridW,0:gridH].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] ])
#template = cv2.imread('template.jpg',0)
key = -1
while key!=27:
ret, img = cap.read()
gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
# Find the chess board corners
ret, corners2 = getCornersChessBoard(gray)
key = cv2.waitKey(100)
# If found, add object points, image points (after refining them)
if ret == True:
objpoints.append(objp)
cv2.cornerSubPix(gray,corners2,(13,13),(-1,-1),criteria)
imgpoints.append(corners2)
# Find the rotation and translation vectors.
rvecs, tvecs, inliers = cv2.solvePnPRansac(objp, corners2, mtx, dist)
reprojectionError(rvecs,tvecs,mtx,dist)
# project 3D points to image plane
imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)
img = draw(img,corners2,imgpts)
print 'CamPos: ',tvecs, ' CamOr: ', (rvecs*180/math.pi)
cv2.imshow('img',img)
exit()
def get_cam2grid(self):
"""Extract grid information from image and generate result image.
Extract translation and rotation of grid from camera. Draw grid corners
on result image. Draw grid pose on result image. Return camera to grid
transformation matrix.
Returns: 6 member list, translation matrix
Raises:
RuntimeError: Could not find a grid
"""
# Get new image
self.image = self.cam.capture_image()
# Find chessboard corners.
re_projection_error, corners = cv2.findChessboardCorners(
self.image, (self. rows, self.cols),
flags=cv2.CALIB_CB_FAST_CHECK + cv2.CALIB_CB_ADAPTIVE_THRESH)
if not re_projection_error:
raise RuntimeError('unable to find grid')
corners2 = cv2.cornerSubPix(self.image, corners, (11, 11),
(-1, -1),
criteria)
if corners2 is None:
corners2 = corners
# Find the rotation and translation vectors.
rvecs, tvecs, inliers = cv2.solvePnPRansac(self.object_point,
corners2,
self.intrinsic,
self.distortion)
# project 3D points to image plane
image_points, jac = cv2.projectPoints(self.axis, rvecs, tvecs,
self.intrinsic,
self.distortion)
self.result_image = cv2.cvtColor(self.image,
cv2.COLOR_GRAY2RGB)
temp_image = cv2.drawChessboardCorners(self.result_image,
(self.cols, self.rows),
corners2,
re_projection_error)
# OpenCV 2 vs 3
if temp_image is not None:
self.result_image = temp_image
self.result_image = draw_axes(self.result_image, corners2,
image_points)
return (np.concatenate((tvecs, rvecs), axis=0)).ravel().tolist()
def detect_glyph(self, image):
"""
returns the rvecs and the tvecs of an image
"""
# global variables for OpenCV tracking
global camera
global contour
global center
# format image for tracking
frame = imutils.resize(image, width = 750)
frame = cv2.flip(frame, 0)
#frame = cv2.flip(frame, 1)
# color space
blueLower = np.array([90,100,10])
blueUpper = np.array([150,255,255])
blackLower = np.array([0,0,0])
blackUpper = np.array([180, 255, 150])
hsv_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
# construct a mask for the color "blue", then remove any imperfections
mask_blue = cv2.inRange(hsv_frame, blueLower, blueUpper)
mask_blue = cv2.erode(mask_blue, None, iterations=1)
mask_blue = cv2.dilate(mask_blue, None, iterations=1)
## create black mask for tracking corner
mask_black = cv2.inRange(hsv_frame, blackLower, blackUpper)
# creates information about the contours`
contour_information = cv2.findContours(mask_blue.copy(), cv2.RETR_CCOMP,
cv2.CHAIN_APPROX_SIMPLE)
# updates each of the elements in the classes
contour.update_contours(contour_information)
center.update_centers(contour.contour_list, mask_black)
if len(center.corners) == 4:
center.reorganize_centers()
center.update_vectors()
center.bool_is_tracking()
# if the camera detects our tracker, it will return the rvecs and tvecs of the image
if center.is_tracking:
rvecs, tvecs, inliers = cv2.solvePnPRansac(camera.objp, np.array(center.final_corners,
dtype = np.float32), camera.mtx, camera.dist)
# if not, return None
else:
rvecs = tvecs = None
return rvecs, tvecs
def get_pose(self, object_points, image_points, ransac=False):
ip = np.array(image_points, np.float32)
op = np.array(object_points, np.float32)
# op_3d = np.array([o + [0.0] for o in object_points], np.float32)
# flag = cv2.CV_P3P
flag = cv2.CV_ITERATIVE
if ransac:
rvec, tvec, inliers = cv2.solvePnPRansac(op, ip, self.cm, self.dc, flags=flag)
return rvec, tvec
else:
ret, rvec, tvec = cv2.solvePnP(op, ip, self.cm, self.dc, flags=flag)
return rvec, tvec
def find_current_pose(self, object_points, intrinsics):
"""
Find camera pose relative to object using current image point set,
object_points are treated as world coordinates
"""
success, rotation_vector, translation_vector = cv2.solvePnPRansac(object_points, self.current_image_points,
intrinsics.intrinsic_mat,
intrinsics.distortion_coeffs,
flags=cv2.SOLVEPNP_ITERATIVE)[0:3]
if success:
self.poses.append(Pose(rotation=rotation_vector, translation_vector=translation_vector))
else:
self.poses.append(None)
return success
def process_image(self, img):
if self.info is None:
self.process_data(None)
return
image_detect = self.detect(img)
if len(image_detect.kp) < self.min_points:
self.process_data(None)
return img
matches = self.match(image_detect)
matched_image_detect = self.filter_train_kp(
image_detect, matches)
matched_pattern_detect = self.filter_query_kp(
self.pattern_detect, matches)
if len(matches) < self.min_points:
self.process_data(None)
return self.draw_match(img, image_detect, matched_image_detect,
success=False)
transform, mask = cv2.findHomography(
np.float32(map(attrgetter('pt'), matched_pattern_detect.kp)),
np.float32(map(attrgetter('pt'), matched_image_detect.kp)),
cv2.RANSAC, 5.0)
bbox = cv2.perspectiveTransform(self.target_points_2d, transform)
if not self.check_match(bbox):
self.process_data(None)
return self.draw_match(img, image_detect, matched_image_detect,
bbox, success=False)
camera_matrix = np.float32(self.info.K).reshape(3, 3)
camera_distortion = np.float32(self.info.D)
rot, trans, inl = cv2.solvePnPRansac(
self.target_points_3d, np.float32(bbox),
camera_matrix, camera_distortion,
iterationsCount=5,
flags=cv2.ITERATIVE,
reprojectionError=20
)
self.process_data(trans, img)
return self.draw_match(img, image_detect, matched_image_detect, bbox,
success=True)
def callback(data):
im = bridge.imgmsg_to_cv2(data, desired_encoding='bgr8')
# im = v.undistort(im, MTX, DIST)
found, corners = find_chessboard_corners(im, SCALE)
if found:
message = BoardMessage()
# rotation and translation of board relative to camera
rvec, tvec, _ = v.solvePnPRansac(OBJP, corners, MTX, DIST)
# grab & reformat the corners and get the correct order for them
outer, _ = v.projectPoints(OUTER, rvec, tvec, MTX, DIST)
outer = np.float32(outer.reshape((4,2)))
order = corner_order(outer)
# undistort the image and put it in the message
M = v.getPerspectiveTransform(outer[order], DSTPTS)
un = v.warpPerspective(im, M, IMSIZE)
message.unperspective = bridge.cv2_to_imgmsg(un, encoding='bgr8')
message.unperspective.header.stamp = rospy.Time.now()
if PUBLISH_UNDISTORT:
impub.publish(message.unperspective)
R, _ = v.Rodrigues(rvec)
G = np.hstack([R, tvec.reshape((3,1))])
outer_c = OUTERH[order].dot(G.T)
print '\n\n==============={}================='.format(video_lock)
fields = 'topleft topright botleft botright'.split()
for i in range(4):
point = PointStamped()
point.point = Point()
point.point.x, point.point.y, point.point.z = outer_c[i]
point.header = Header()
point.header.frame_id = 'left_hand_camera'
point.header.stamp = rospy.Time(0)
p = tfl.transformPoint(BASE_FRAME, point).point
setattr(message, fields[i], p)
print [p.x, p.y, p.z]
if video_lock != 1:
pub.publish(message)
if PUBLISH_DRAWPOINTS:
v.drawChessboardCorners(im, (7,7), corners, found)
ptpub.publish(bridge.cv2_to_imgmsg(im, encoding='bgr8'))
def pnp_test(key, xyz, angs):
cam = pu.CameraHelper()
_x, _y, _z = xyz
_azi, _ele = angs
cam_xyz = np.float32([_x, _y, _z])
# world landmark positions
xyz1_o = mark1[key].xyz
xyz2_o = mark2[key].xyz
xyz3_o = mark3[key].xyz
xyzb_o = markb[key].xyz
# rotate and offset landmark positions as camera will see them
xyz1_rot = pu.calc_xyz_after_rotation_deg(xyz1_o - cam_xyz, _ele, _azi, 0)
xyz2_rot = pu.calc_xyz_after_rotation_deg(xyz2_o - cam_xyz, _ele, _azi, 0)
xyz3_rot = pu.calc_xyz_after_rotation_deg(xyz3_o - cam_xyz, _ele, _azi, 0)
xyzb_rot = pu.calc_xyz_after_rotation_deg(xyzb_o - cam_xyz, _ele, _azi, 0)
# project them to camera plane
uv1 = cam.project_xyz_to_uv(xyz1_rot)
uv2 = cam.project_xyz_to_uv(xyz2_rot)
uv3 = cam.project_xyz_to_uv(xyz3_rot)
uvb = cam.project_xyz_to_uv(xyzb_rot)
if cam.is_visible(uv1) and cam.is_visible(uv2) and cam.is_visible(uv3) and cam.is_visible(uvb):
objectPoints = np.array([xyz1_o, xyz2_o, xyz3_o, xyzb_o])
imagePoints = np.array([uv1, uv2, uv3, uvb])
rvecR, tvecR, inliers = cv2.solvePnPRansac(objectPoints, imagePoints, cam.camA, cam.distCoeff)
if inliers is not None:
newImagePoints, _ = cv2.projectPoints(objectPoints, rvecR, tvecR, cam.camA, cam.distCoeff)
# print newImagePoints
rotM, _ = cv2.Rodrigues(rvecR)
q = -np.matrix(rotM).T * np.matrix(tvecR)
print q
else:
print "*** PnP failed ***"
else:
print "a PnP coord is not visible"
def getProperties(points):
"""
* Function Name: getProperties
* Input: A set of four points of the corners of aruco markers.
These points can be obtained from is_aruco_present()
function.
* Output: -
* Logic: The Perspective-n-Point problem is solved by Ransac
algorithm. We obtain the translation and rotation
vectors through this function.
* Example Call: getProperties(points)
"""
global objp
# Arrays to store object points and image points from all the images.
objpoints = objp
print "OBJP", objpoints
imgpoints = points
#imgpoints = np.array(imgpoints)
print "IMGP", imgpoints
mtx = np.load('matrix.npy')
dist = np.load('distortion.npy')
rvec, tvec, inliers = cv2.solvePnPRansac(objpoints, imgpoints, mtx, dist)
print "Rvec\n", rvec
print "\nTvec", tvec
x = tvec[0][0]
y = tvec[2][0]
dst, jacobian = cv2.Rodrigues(rvec)
print "Rot Matrix", dst
t = math.asin(-dst[0][2])
t1 = math.acos(dst[0][0])
return x, y, t, t1
def get_vectors(image, points, mtx, dist):
# order points
points = order_points(points)
# 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")
# calculate rotation and translation vectors
cv2.cornerSubPix(gray,imgp,(11,11),(-1,-1),criteria)
rvecs, tvecs, _ = cv2.solvePnPRansac(objp, imgp, mtx, dist)
return rvecs, tvecs
def HeadPoseEstimation(self,camera,screen):
point2d=np.ascontiguousarray(self.point2d.reshape((self.point2d.shape[0],2,1)))
point3d=np.ascontiguousarray(self.point3d.reshape((self.point3d.shape[0],3,1)))
# a,b,c=cv2.solvePnP(point3d,point2d,cameraMatrix=camera.K,distCoeffs=camera.D)
_,b,c,a=cv2.solvePnPRansac(point3d,point2d,cameraMatrix=camera.K,distCoeffs=camera.D)
print(c)
headT = c.reshape((3, 1))
headR, _ = cv2.Rodrigues(b)
transform = np.array([[1, 0, 0], [0, 1, 0], [0, 0, -1]])
camera.R_inHead=headR
camera.t_inHead=headT
self.R_inCam=np.linalg.inv(headR)
self.t_inCam=-np.matmul(self.R_inCam,headT)
Rheads = np.matmul(np.matmul(screen.R_inCam, transform), headR)
Theads = np.matmul(np.matmul(screen.R_inCam, transform), headT) +screen.t_inCam
self.R_inScreen = np.linalg.inv(Rheads)
self.t_inScreen = -np.matmul(self.R_inScreen, Theads)
landmark3dInCam=self.point3d
for i in range(self.point3d.shape[0]):
landmark3dInCam[i]=(np.matmul(self.R_inScreen,landmark3dInCam[i].reshape((3,1)))+self.t_inScreen).reshape((1,3))
return landmark3dInCam
def reposition(self,opts,ipts,flag=cv2.CV_P3P,ransac=0,err=8):
'''
Redefines the position vectors r and t of the camera given a set of
corresponding image and object points
Options:
-guess for camera position (using rvguess function)
-method: Iterative = 0, EPNP = 1, P3P = 2
-ransac method for solvepnp
'''
rvec = 0
tvec = 0
if flag == cv2.CV_P3P or flag == cv2.CV_EPNP:
n_points = ipts.shape[0]
ipts = np.ascontiguousarray(ipts[:,:2]).reshape((n_points,1,2))
if ransac:
rvec,tvec,result = cv2.solvePnPRansac(opts,ipts,self.C,self.dist,
flags=flag,reprojectionError=err)
else:
result,rvec,tvec = cv2.solvePnP(opts,ipts,self.C,self.dist,
self.r,self.t,0,flag)
self.r = np.matrix(rvec)
self.t = np.matrix(tvec)
self.update()
return result
def get_vectors(image, points):
# order points
points = order_points(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")
# calculate rotation and translation vectors
cv2.cornerSubPix(gray,imgp,(11,11),(-1,-1),criteria)
rvecs, tvecs, _ = cv2.solvePnPRansac(objp, imgp, mtx, dist)
return rvecs, tvecs
def resect(data, graph, reconstruction, shot_id):
'''Add a shot to the reconstruction.
'''
xs = []
Xs = []
for track in graph[shot_id]:
if track in reconstruction['points']:
xs.append(graph[shot_id][track]['feature'])
Xs.append(reconstruction['points'][track]['coordinates'])
x = np.array(xs)
X = np.array(Xs)
if len(x) < 5:
return False
exif = data.load_exif(shot_id)
camera_model = exif['camera']
K = multiview.K_from_camera(reconstruction['cameras'][camera_model])
dist = np.array([0,0,0,0.])
# Prior on focal length
R, t, inliers = cv2.solvePnPRansac(X.astype(np.float32), x.astype(np.float32), K, dist,
reprojectionError=data.config.get('resection_threshold', 0.004))
if inliers is None:
print 'Resection', shot_id, 'no inliers'
return False
print 'Resection', shot_id, 'inliers:', len(inliers), '/', len(x)
if len(inliers) >= data.config.get('resection_min_inliers', 15):
reconstruction['shots'][shot_id] = {
"camera": camera_model,
"rotation": list(R.flat),
"translation": list(t.flat),
}
add_gps_position(data, reconstruction['shots'][shot_id], shot_id)
return True
else:
return False
ax = plt.axes(projection='3d')
ax.plot_surface(c[0::],c[1::],c[2::]),plt.show()
"""
# print c.shape
# x = np.delete(c, np.s_[1:], 2)
# x = x.reshape(c.shape[0],c.shape[1])
# print x.shape
objPts = np.array([[1, 2, 3]], dtype=np.float32)
imgPts = np.array([[1, 2]], dtype=np.float32)
# finding the rotation and translation vectors
# params: ObjectPoints , ImagePoint
rvecs, tvecs, _ = cv2.solvePnPRansac(objPts, imgPts, mtx, dist)
# project 3D points to image plane
imgpts, jaccard = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)
img = draw(c, [np.array(center)], imgpts)
# only proceed if the radius meets a minimum size
if radius > 10:
# draw the circle and centroid on the frame,
# then update the list of tracked points
cv2.circle(frame, (int(x), int(y)), int(radius), (0, 255, 255), 2)
cv2.circle(frame, center, 5, (0, 0, 255), -1)
# update the points queue
pts.appendleft(center)
img = "Marker_sample.jpg"
pts, flag = Points(img)
if flag == '1':
imgp = Perspective(pts, img)
elif flag == '2':
imgp = crop(pts, img)
elif flag == '-1':
quit()
print "Image points\n\n", imgp
mtx = np.load('matrix.npy')
dist = np.load('distortion.npy')
rvec, tvec, inliers = cv2.solvePnPRansac(objp, imgp, mtx, dist)
print "Rvec\n", rvec
print "\nTvec", tvec
dst, jacobian = cv2.Rodrigues(rvec)
x = tvec[0][0]
y = tvec[2][0]
t = (math.asin(-dst[0][2]))
print "X", x, "Y", y, "Angle", t
print "90-t", (math.pi/2) - t
Rx = y * (math.cos((math.pi/2) - t))
Ry = y * (math.sin((math.pi/2) - t))
# img = cv2.line(img, corner, tuple(img_pts[1].ravel()), (0, 255, 0), 5)
# img = cv2.line(img, corner, tuple(img_pts[2].ravel()), (0, 0, 255), 5)
for i in range(np.shape(img_pts)[0]):
# img = cv2.line(img, tuple(corners[i].ravel()), tuple(img_pts[i].ravel()), (255, 255, 0), 5)
# util.img.circle(img, img_pts[i].ravel(), r = 11, color = util.img.COLOR_BGR_RED)
right = i + 1
down = i + board.n_cols
if right < len(img_pts) and right // board.n_cols == i // board.n_cols:
img = cv2.line(img, tuple(img_pts[i].ravel()),
tuple(img_pts[right].ravel()), (255, 0, 0), 5)
img = cv2.line(img, tuple(corners[i].ravel()),
tuple(corners[right].ravel()), (255, 0, 0), 5)
if down < len(img_pts):
img = cv2.line(img, tuple(img_pts[i].ravel()),
tuple(img_pts[down].ravel()), (255, 0, 0), 5)
img = cv2.line(img, tuple(corners[i].ravel()),
tuple(corners[down].ravel()), (255, 0, 0), 5)
return img
# img = board.draw_corners(img.copy(), corners = corners)
ok, corners = board.find_corners(img)
_, R, t, inliners = cv2.solvePnPRansac(obj_points, corners,
camera_model.intrinsics,
camera_model.distortion)
obj_points[:, :, -1] = -3
img_pts, jac = cv2.projectPoints(obj_points, R, t, camera_model.intrinsics,
camera_model.distortion)
img = draw(img, corners, img_pts, idx=3)
util.img.imshow("", img)
def pnp(kxyz, kxy, fov, dims, ransac=True, **kwds):
"""
Solves the pnp problem to locate a camera given keypoints that are known in world and pixel coordinates using opencv.
*Arguments*:
- kxyz = Nx3 array of keypoint positions in world coordinates.
- kxy = Nx2 array of corresponding keypoint positions in pixel coordinates.
- fov = the (vertical) field of view of the camera.
- dims = the dimensions of the image in pixels
- ransac = true if ransac should be used to filter outliers. Default is True.
*Keywords*:
- optical_centre = the pixel coordinates (cx,cy) of the optical centre to use. By default the
middle pixel of the image is used.
- other keyword arguments are passed to cv2.solvePnP(...) or cv2.solvePnPRansac(...).
Additionally a custom optical center can be passed as (cx,cy)
*Returns*:
- p = the camera position in world coordinates
- r = the camera orientation (as XYZ euler angle).
- inl = list of Ransac inlier indices used to estimate the position, or None if ransac == False.
"""
# normalize keypoints so that origin is at mean
mean = np.mean(kxyz, axis=0)
kxyz = kxyz - mean
# flip kxy coords to match opencv coord system
# kxy[:, 1] = dims[1] - kxy[:, 1]
# kxy[:, 0] = dims[0] - kxy[:, 0]
# compute camera matrix
tanfov = np.tan(np.deg2rad(fov / 2))
aspx = dims[0] / dims[1]
fx = dims[0] / (2 * tanfov * aspx)
fy = dims[1] / (2 * tanfov)
cx, cy = kwds.get("optical_centre", (0.5 * dims[0] - 0.5, 0.5 * dims[1] - 0.5))
if 'optical_centre' in kwds: del kwds['optical_centre'] # remove keyword
cameraMatrix = np.array([[fx, 0, cx],
[0, fy, cy],
[0, 0, 1]])
# distortion free lens
dist_coef = np.zeros(4)
# use opencv to solve pnp problem and hence camera position
if ransac:
suc, rot, pos, inl = cv2.solvePnPRansac(objectPoints=kxyz[:, :3].copy(), # points in world coords
imagePoints=kxy[:, :2].copy(), # points in img coords
cameraMatrix=cameraMatrix, # camera matrix
distCoeffs=dist_coef,
**kwds)
else:
inl = None
suc, rot, pos = cv2.solvePnP(objectPoints=kxyz[:, :3].copy(), # points in world coords
imagePoints=kxy[:, :2].copy(), # points in img coords
cameraMatrix=cameraMatrix, # camera matrix
distCoeffs=dist_coef,
**kwds)
assert suc, "Error - no solution found to pnp problem."
# get first solution
if not inl is None:
inl = inl[:, 0]
pos = np.array(pos[:, 0])
rot = np.array(rot[:, 0])
# apply rotation position vector
p = -np.dot(pos, spatial.transform.Rotation.from_rotvec(rot).as_matrix())
# convert rot from axis-angle to euler
r = spatial.transform.Rotation.from_rotvec(rot).as_euler('xyz', degrees=True)
# apply dodgy correction to r (some coordinate system shift)
r = np.array([r[0] - 180, -r[1], -r[2]])
return p + mean, r, inl
def calibration_opencv(self,rgb,h,w,sq_size,use_pnp = True,use_ransac = True):
self.opencv_th.set()
#cameraMatrix = self.kinect.rgb_camera_info.K.reshape(3,3)
projMatrix = np.matrix(self.kinect.rgb_camera_info.P).reshape(3,4)
cameraMatrix, rotMatrix, transVect, rotMatrixX, rotMatrixY, rotMatrixZ, eulerAngles = cv2.decomposeProjectionMatrix(projMatrix)
distCoeffs = np.array(self.kinect.rgb_camera_info.D)
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.1)
gray = cv2.cvtColor(rgb,cv2.COLOR_BGR2GRAY)
objp = np.zeros((h*w,3), np.float32)
objp[:,:2] = np.mgrid[0:h,0:w].T.reshape(-1,2)
objp = objp*sq_size
objp[:,[0, 1]] = objp[:,[1, 0]]
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.
# Find the chess board corners
ret, corners = cv2.findChessboardCorners(gray, (h,w),None)
# If found, add object points, image points (after refining them)
if ret == True:
objpoints.append(objp)
cv2.cornerSubPix(gray,corners,(5,5),(-1,-1),criteria)
imgpoints.append(corners)
# Draw and display the corners
cv2.drawChessboardCorners(rgb, (h,w), corners,ret)
if use_pnp:
if use_ransac:
if not self.useExtrinsicGuess:
rvec,tvec,_ = cv2.solvePnPRansac(objp, corners,cameraMatrix ,distCoeffs)
self.rvec = rvec
self.tvec = tvec
self.useExtrinsicGuess = True
else:
r,t,_ = cv2.solvePnPRansac(objp, corners,cameraMatrix ,distCoeffs,self.rvec, self.tvec, self.useExtrinsicGuess)
rvec = self.rvec
tvec = self.tvec
else:
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1],cameraMatrix,None,flags=cv2.CALIB_USE_INTRINSIC_GUESS)#flags=cv2.CALIB_FIX_ASPECT_RATIO)
rvec = rvecs[0]
tvec = tvecs[0]
tvect = np.matrix(tvec).reshape(3,1)
imgpoints2, _ = cv2.projectPoints(objp, rvec, tvec, cameraMatrix, distCoeffs)
for p in imgpoints2:
cv2.circle(rgb,(int(p[0][0]),int(p[0][1])),2,(0,12,235),1)
ret_R,_ = cv2.Rodrigues(rvec)
#print tvec,rvec
ret_Rt = np.matrix(ret_R)
tmp = np.append(ret_Rt, np.array([0,0,0]).reshape(3,1), axis=1)
aug=np.array([[0.0,0.0,0.0,1.0]])
T = np.append(tmp,aug,axis=0)
quaternion = quaternion_from_matrix(T)
self.tfcv_thread.set_transformation(tvect,quaternion)
cv2.imshow('findChessboardCorners - OpenCV',rgb)
self.opencv_th.clear()
return
objp2 = np.zeros((6 * 7, 3), np.float32)
objp2[:, :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('/home/airscan/stereo/calib_imgs/left*.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.
_, r, t, inliers = cv2.solvePnPRansac(
objp2, corners2, mtx, dist) #changed from rvecs to r etc
print(r)
print(t)
# project 3D points to image plane
imgpts, jac = cv2.projectPoints(axis, r, t, mtx, dist)
img = draw(img, corners2, imgpts)
cv2.imshow('img', img)
k = cv2.waitKey(0) & 0xff
if k == 's':
cv2.imwrite(fname[:6] + '.png', img)
cv2.destroyAllWindows()
def fusion(output, width, height, intrinsics, conf_thresh, batchIdx, bestCnt,
rawimg, seg_save_path):
layerCnt = len(output)
assert (layerCnt >= 2)
inlierImg = np.copy(rawimg)
cls_confs = output[0][0][batchIdx]
cls_ids = output[0][1][batchIdx]
predx = output[1][0][batchIdx]
predy = output[1][1][batchIdx]
det_confs = output[1][2][batchIdx]
keypoints = output[1][3]
nH, nW, nV = predx.shape
nC = cls_ids.max() + 1
outPred = []
p2d = None
repro_dict = {}
mx = predx.mean(axis=2) # average x positions
my = predy.mean(axis=2) # average y positions
mdConf = det_confs.mean(axis=2) # average 2D confidences
for cidx in range(nC): # loop for every class
# skip background
if cidx == 0:
continue
foremask = (cls_ids == cidx)
cidx -= 1
foreCnt = foremask.sum()
if foreCnt < 1:
continue
xs = predx[foremask]
ys = predy[foremask]
ds = det_confs[foremask]
cs = cls_confs[foremask]
centerxys = np.concatenate(
(mx[foremask].reshape(-1, 1), my[foremask].reshape(-1, 1)), 1)
# choose the item with maximum detection confidence
# actually, this will choose only one object instance for each type, this is true for OccludedLINEMOD and YCB-Video dataset
maxIdx = np.argmax(mdConf[foremask])
refxys = centerxys[maxIdx].reshape(1, -1).repeat(foreCnt, axis=0)
selected = (np.linalg.norm(centerxys - refxys, axis=1) < 0.2)
xsi = xs[selected] * width
ysi = ys[selected] * height
dsi = ds[selected]
csi = cs[selected] # confidence of selected points
if csi.mean() < conf_thresh: # valid classification probability
continue
gridCnt = len(xsi)
assert (gridCnt > 0)
# choose best N count, here N = bestCnt (default = 10)
p2d = None
p3d = None
candiBestCnt = min(gridCnt, bestCnt)
for i in range(candiBestCnt):
bestGrids = dsi.argmax(axis=0)
validmask = (dsi[bestGrids, list(range(nV))] > 0.5)
xsb = xsi[bestGrids, list(range(nV))][validmask]
ysb = ysi[bestGrids, list(range(nV))][validmask]
t2d = np.concatenate((xsb.reshape(-1, 1), ysb.reshape(-1, 1)), 1)
t3d = keypoints[cidx][validmask]
if p2d is None:
p2d = t2d
p3d = t3d
else:
p2d = np.concatenate((p2d, t2d), 0)
p3d = np.concatenate((p3d, t3d), 0)
dsi[bestGrids, list(range(nV))] = 0
if len(p3d) < 6:
continue
# DEBUG: show the selected 2D reprojections
if True:
for i in range(len(p2d)):
x = p2d[i][0]
y = p2d[i][1]
inlierImg = cv2.circle(inlierImg, (int(x), int(y)), 2,
get_class_colors(cidx), -1)
retval, rot, trans, inliers = cv2.solvePnPRansac(
p3d, p2d, intrinsics, None, flags=cv2.SOLVEPNP_EPNP)
if not retval:
continue
R = cv2.Rodrigues(rot)[0] # convert to rotation matrix
T = trans.reshape(-1, 1)
rt = np.concatenate((R, T), 1)
# DEBUG: compute predicted points in pixel after reprojection ( note: 8, the number of keypoints, is hardcoded )
pred_kp = vertices_reprojection(p3d, rt, intrinsics)
pred_kp = pred_kp[:8, :]
if pred_kp.shape[0] == 8:
repro_dict[cidx + 1] = pred_kp
outPred.append(
[cidx, rt, 1, None, None, None, [cidx], -1, [0], [0], None])
# save image of predicted keypoints with best confidence (before reporojection)
if seg_save_path:
points_path = seg_save_path[:-4] + "-points.jpg"
print("save predicted points to ", points_path)
cv2.imwrite(points_path, inlierImg)
return outPred, p2d, repro_dict
def _solve_pnp(self):
"""
Driver Code for solving Perspective-n-Point (PnP) problem. The module uses OpenCV's implementation of P3P algorithm
and RANSAC to retrieve the relative rotation and translation vector (used Rodrigues formula to get rotation matrix) between the world (previous state)
and camera (current state) in the camera coordinate frame. Module further processes to obtain the relative rotation and
translation in the world coordinate frame (camera coordinate frame of previous state). Also,
Return:
:r_mat (np.array) : size(3,3) : relative rotation in world coordinate frame (previous stereo state)
:t_vec (np.array) : size(3,1) : relative translation in world coordinate frame (previous stereo state)
"""
# Prepare argument for PnP solver
args_pnpSolver = self.params.geometry.pnpSolver
for i in range(args_pnpSolver.numTrials):
# Obtain r_vec and t_vec in camera coordinate frames (currState)
_, r_vec, t_vec, idxPose = cv2.solvePnPRansac(
self.prevState.pts3D_Tracking,
self.currState.pointsTracked.left,
self.intrinsic,
None,
iterationsCount=args_pnpSolver.numTrials,
reprojectionError=args_pnpSolver.reprojectionError,
confidence=args_pnpSolver.confidence,
flags=cv2.SOLVEPNP_P3P)
# Use Rodrigues formaula (SO3) to obtain rotational matrix
r_mat, _ = cv2.Rodrigues(r_vec)
# Convert relative roation and translation in the world coordiante frame (prevState)
t_vec = -r_mat.T @ t_vec
r_mat = r_mat.T
# Prepare index to retrieve inliers on the current traced points and 3D points
try:
idxPose = idxPose.flatten()
except:
import ipdb
ipdb.set_trace()
# Ensure we get enough inliers from the PnP problem
'''To Do: Add logger object to record the terminal output'''
ratio = len(idxPose) / len(self.prevState.pts3D_Tracking)
scale = np.linalg.norm(t_vec)
if scale < args_pnpSolver.deltaT and ratio > args_pnpSolver.minRatio:
# print("Scale of translation of camera : {}".format(scale))
# print("Solution obtained in P3P Iteration : {}".format(i+1))
# print("Ratio of Inliers : {}".format(ratio))
break
else:
pass
# print("Warning : Max Iter : {} reached, still large position delta produced".format(i))
# Remove outliers from tracked points and triangulated and filtered 3D world coordinate points
self.currState.pointsTracked = (
self.currState.pointsTracked.left[idxPose],
self.currState.pointsTracked.right[idxPose])
self.prevState.P3P_pts3D = self.prevState.pts3D_Tracking[idxPose]
return r_mat, t_vec
def estimate_new_view_pose(self, img):
descriptors = [
uImg['descriptors'] for uImg in self.img_data[:self.imgs_used - 1]
]
# descriptors = [self.img_data[self.imgs_used-1]['descriptors']]
# for uImg in self.img_data[:self.imgs_used]:
# descriptors.append(uImg['descriptors'])
prev_pose = self.img_data[self.imgs_used - 1]['pose']
matches = self.trainFlannMatch(img, descriptors)
# matches = self.kNNMatch(self.img_data[self.imgs_used-1], img)
# 3d Points
points_3d = []
points_2d = []
for m in matches:
# clouds are made of image pairs so (0,1) -> cloud_idx:0 (1,2) -> cloud_idx:1, ...
cloud_idx = m.imgIdx
#if m.imgIdx != 0:
# cloud_idx = m.imgIdx-1
pointIdx = np.searchsorted(
self.point_cloud[cloud_idx]['point_img_corresp'], m.trainIdx)
if pointIdx >= len(
self.point_cloud[cloud_idx]['point_img_corresp']):
continue
# Get the 3d Point corresponding to the train image keypoint
points_3d.append(self.point_cloud[cloud_idx]['3dpoints'][pointIdx])
# 2d Points
x_coords = int(img['keypoints'][m.queryIdx].pt[0])
y_coords = int(img['keypoints'][m.queryIdx].pt[1])
points_2d.append([x_coords, y_coords])
#camera_pose = np.zeros((3,4))
# estimate camera pose from 3d2d Correspondences
if len(points_3d) > 4 and len(points_2d) > 4:
rvec_in, _ = cv.Rodrigues(prev_pose[:, :3])
tvec_in = prev_pose[:, 3]
# _, rvec, tvec = cv.solvePnP(
# np.array(points_3d, dtype=np.float64),
# np.array(points_2d, dtype=np.float64),
# self.K, self.distCoeffs, flags=cv.SOLVEPNP_ITERATIVE,
# # useExtrinsicGuess=True, rvec=rvec_in, tvec=tvec_in)
_, rvec, tvec, inliers = cv.solvePnPRansac(
np.array(points_3d, dtype=np.float64),
np.array(points_2d, dtype=np.float64),
self.K,
self.distCoeffs,
confidence=0.999,
flags=cv.SOLVEPNP_P3P,
useExtrinsicGuess=True,
rvec=rvec_in,
tvec=prev_pose[:, 3],
reprojectionError=3.0)
# LMEDS PnP PnPRansac
# ITERATIVE 8,0 X
# P3P X 7,0
# EPNP X X
# DLS X X
# UPNP X X
# AP3P X 5,0
# MAX_COUNT X X
# RANSAC PnP PnPRansac
# ITERATIVE 8,0 5,0
# P3P X 7,0
# EPNP X 4,0
# DLS X 4,0
# UPNP X 4,0
# AP3P X 7,0
# MAX_COUNT X X
# 8POINT PnP PnPRansac
# ITERATIVE X X
# P3P X X
# EPNP X X
# DLS X X
# UPNP X X
# AP3P X X
# MAX_COUNT X X
cam_rmat, _ = cv.Rodrigues(rvec)
camera_pose = np.concatenate([cam_rmat.get(), tvec.get()], axis=1)
#camera_pose = np.concatenate([cam_rmat, tvec], axis=1)
else:
camera_pose = np.zeros((3, 4))
# camera_pose[:,:3] = cam_rmat
# camera_pose[:,3] = tvec.T
return camera_pose
#print("tvecs pnp", tvecs)
# project 3D points to image plane
imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, camera_matrix,
dist_coefs)
#getting rotation matrix
rmat = cv2.Rodrigues(rvecs)[0]
print("this is rotation matrix from pnp ", rmat)
print("this is tvecs", tvecs)
#sovlve pnpransac
_, rvecss, tvecss, inliers = cv2.solvePnPRansac(objp, corners2,
camera_matrix, dist_coefs)
print("rvecss", rvecss)
print("tvecss", tvecss)
tarray = np.array(tvecss).T
print('tvecss', tarray)
dis = cv2.Rodrigues(rvecs)[0]
#printing rotation matrxi name dis with PnPRansac
print('this is rotation matrix from ransac', dis)
x = tvecss[0][0]
y = tvecss[2][0]
t = (math.asin(-dis[0][2]))
def forward(ctx, pts2d, pts3d, K, ini_pose=None, prevent_deteriorate=True):
bs = pts2d.size(0)
n = pts2d.size(1)
device = pts2d.device
pts3d_np = np.array(pts3d.detach().cpu())
K_np = np.array(K.cpu())
P_6d = torch.zeros(bs, 6, device=device)
for i in range(bs):
pts2d_i_np = np.ascontiguousarray(pts2d[i].detach().cpu()).reshape(
(n, 1, 2))
_, rvec0r, T0r, _ = cv.solvePnPRansac(objectPoints=pts3d_np,
imagePoints=pts2d_i_np,
cameraMatrix=K_np,
distCoeffs=None,
flags=cv.SOLVEPNP_ITERATIVE,
confidence=0.9999,
reprojectionError=20)
angle_axis0r = torch.tensor(rvec0r,
device=device,
dtype=torch.float).view(1, 3)
tra0r = torch.tensor(T0r, device=device,
dtype=torch.float).view(1, 3)
P_6d_0r = torch.cat((angle_axis0r, tra0r), dim=-1)
res0r, feas0r = get_res(pts2d[i], pts3d, K, P_6d_0r)
if ini_pose is None:
rvec0 = rvec0r
T0 = T0r
P_6d_i_before = P_6d_0r
res0 = res0r
feas0 = feas0r
else:
res0i, feas0i = get_res(pts2d[i], pts3d, K,
ini_pose[i].view(1, 6))
if feas0r and not feas0i:
rvec0 = rvec0r
T0 = T0r
P_6d_i_before = P_6d_0r
res0 = res0r
feas0 = feas0r
elif feas0i and not feas0r:
rvec0 = np.array(ini_pose[i, 0:3].cpu().view(3, 1))
T0 = np.array(ini_pose[i, 3:6].cpu().view(3, 1))
P_6d_i_before = ini_pose[i].view(1, 6)
res0 = res0i
feas0 = feas0i
else:
if res0i < res0r:
rvec0 = np.array(ini_pose[i, 0:3].cpu().view(3, 1))
T0 = np.array(ini_pose[i, 3:6].cpu().view(3, 1))
P_6d_i_before = ini_pose[i].view(1, 6)
res0 = res0i
feas0 = feas0i
else:
rvec0 = rvec0r
T0 = T0r
P_6d_i_before = P_6d_0r
res0 = res0r
feas0 = feas0r
_, rvec, T = cv.solvePnP(objectPoints=pts3d_np,
imagePoints=pts2d_i_np,
cameraMatrix=K_np,
distCoeffs=None,
flags=cv.SOLVEPNP_ITERATIVE,
useExtrinsicGuess=True,
rvec=np.copy(rvec0),
tvec=np.copy(T0))
if prevent_deteriorate == True:
angle_axis = torch.tensor(rvec,
device=device,
dtype=torch.float).view(1, 3)
tra = torch.tensor(T, device=device,
dtype=torch.float).view(1, 3)
P_6d_i_after = torch.cat((angle_axis, tra), dim=-1)
res, feas = get_res(pts2d[i], pts3d, K, P_6d_i_after)
if feas0 and not feas:
P_6d[i, :] = P_6d_i_before
elif feas and not feas0:
P_6d[i, :] = P_6d_i_after
else:
if res <= res0:
P_6d[i, :] = P_6d_i_after
else:
P_6d[i, :] = P_6d_i_before
else:
angle_axis = torch.tensor(rvec,
device=device,
dtype=torch.float).view(1, 3)
T = torch.tensor(T, device=device,
dtype=torch.float).view(1, 3)
P_6d[i, :] = torch.cat((angle_axis, T), dim=-1)
ctx.save_for_backward(pts2d, P_6d, pts3d, K)
return P_6d
def run_cv_demo():
global animal
currentZoom = 15
curX = 0
curY = 0
curZ = 0
curRotX = 0
curRotY = 0
curRotZ = 0
anidex = 7
#animal = loadAnimal(anidex)
video = cv2.VideoCapture(1)
ret, mtx, dist, rvecs, tvecs = calibrate(video)
grabbed, frame = video.read()
if not grabbed:
print("failed")
return
height, width, _ = frame.shape
frame_num = 1
test = cv2.imread('checkerboard.jpg', 0)
ret, testcorners = cv2.findChessboardCorners(test, (8, 6), None)
objp = np.zeros((6 * 8, 3), np.float32)
objp[:, :2] = np.mgrid[0:8, 0:6].T.reshape(-1, 2)
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
# Arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.
while True:
flags, hcursor, (x, y) = win32gui.GetCursorInfo()
cv2.imshow('Live Session', frame)
grabbed, frame = video.read()
a, b, x, y, back, start, lt, rt, lb, rb, lt_x, lt_y, rt_x, rt_y = getXbox(
)
key = cv2.waitKey(1) & 0xFF
# terminate session if esc or q was pressed
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, (8, 6), None)
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 found, add object points, image points (after refining them)
if ret == True:
objpoints.append(objp)
corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1),
criteria)
imgpoints.append(corners2)
# Draw and display the corners
frame = cv2.drawChessboardCorners(frame, (8, 6), corners2, ret)
ret, rvecs, tvecs, inliers = cv2.solvePnPRansac(
objp, corners2, mtx, dist)
# project 3D points to image plane
#axis2 = transformPointset(axis, TAAtoTM(np.array([x,y,-4,0,np.pi/2,np.pi/2])))
imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)
#imgpts2, jac2 = cv2.projectPoints(axis2, rvecs, tvecs, mtx, dist)
#frame = draw(frame,corners2,imgpts2)
#try:
camera_parameters = np.array([[800, 0, 320], [0, 800, 240],
[0, 0, 1]])
homography, mask = cv2.findHomography(testcorners, corners,
cv2.RANSAC, 5.0)
projection = projection_matrix(camera_parameters, homography)
#try:
if a == 1:
curRotX = 0
curRotY = 0
curRotZ = 0
currentZoom += rt_y / 5
if lt >= .2 or lt <= -.2:
curRotX += lt_x
curRotY += lt_y
curRotZ += rt_x
else:
curX += lt_x * 5
curY += lt_y * 5
curZ += rt_x * 5
frame = render(
frame, animal, projection, test, currentZoom,
TAAtoTM(np.array([curX, curY, curZ, curRotX, curRotY,
curRotZ])), False)
#except:
# pass
#except:
# pass
if not grabbed or key == 27 or key == ord('q'):
break
def sfm_rec():
image_dir = rec_config.image_dir + '/'
image_names = glob.glob(image_dir + '*.jpg') # 读取图片本身的名字
image_names = sorted(image_names)
with open("../project_name.txt", "r") as f:
project_name = f.read()
# TODO: GUI更改文件夹位置1
# with open("../../project_name.txt", "r") as f:
# project_name = f.read()
K = np.load('../calibration/camera_params/' + project_name + '/K.npy')
# TODO: GUI更改文件夹位置2
# K = np.load('../../calibration/camera_params/' + project_name + '/K.npy')
# 加载畸变系数
params_dir = '../calibration/camera_params/' + project_name
distort_coefs = np.load(params_dir + '/dist.npy')
print('Distortion coefficients:\n', distort_coefs)
# 提取特征点、特征匹配
print('提取所有图片特征点...')
keypoints_for_all, descriptors_for_all, colors_for_all = extract_feathers(
image_names)
# print(colors_for_all)
print('匹配所有图片特征...')
matches_for_all = match_all_feather(keypoints_for_all, descriptors_for_all)
for i in range(len(matches_for_all)):
print(len(matches_for_all[i]), end=' ')
# 初始化点云
print('\n初始化点云...')
structure, correspond_struct_idx, colors, rotations, motions, projections = init_structure(
K, keypoints_for_all, colors_for_all, matches_for_all)
print("初始化点云数目:", len(structure))
# 增量方式添加剩余点云
print('增量方式添加剩余点云...')
for i in tqdm(range(1, len(matches_for_all))):
# 获取第i幅图中匹配点的空间三维坐标，以及第i+1幅图匹配点的像素坐标
obj_points, img_points = get_objpts_and_imgpts(
matches_for_all[i], correspond_struct_idx[i], structure,
keypoints_for_all[i + 1])
# solvePnPRansac得到第i+1个相机的旋转和平移
# 在python的opencv中solvePnPRansac函数的第一个参数长度需要大于7，否则会报错
# 这里对小于7的点集做一个重复填充操作，
if len(obj_points) < 7:
while len(img_points) < 7:
obj_points = np.append(obj_points, [obj_points[0]], axis=0)
img_points = np.append(img_points, [img_points[0]], axis=0)
# 得到第i+1幅图相机的旋转向量和位移矩阵
_, r, T, _ = cv2.solvePnPRansac(obj_points, img_points, K,
np.array([]))
R, _ = cv2.Rodrigues(r) # 将旋转向量转换为旋转矩阵
rotations.append(R)
motions.append(T)
# 根据R T进行重建
p1, p2 = get_matched_points(keypoints_for_all[i],
keypoints_for_all[i + 1],
matches_for_all[i])
c1, c2 = get_matched_colors(colors_for_all[i], colors_for_all[i + 1],
matches_for_all[i])
new_structure, new_proj = reconstruction(K, rotations[i], motions[i],
R, T, p1, p2)
projections.append(new_proj)
# 点云融合
structure, colors, correspond_struct_idx[i], correspond_struct_idx[
i + 1] = fusion_structure(matches_for_all[i],
correspond_struct_idx[i],
correspond_struct_idx[i + 1], structure,
new_structure, colors, c1)
print("新生成点云数", len(new_structure), "第", i, "次融合后点云数", len(structure))
print(len(structure))
# BA优化
print('删除误差较大的点')
structure = delete_error_point(rotations, motions, K,
correspond_struct_idx, keypoints_for_all,
structure)
# 由于经过bundle_adjustment的structure，会产生一些空的点（实际代表的意思是已被删除）
# 修改各图片中关键点的索引
# 修改点云中的None为 -1
for i in range(len(structure)):
if math.isnan(structure[i][0]):
structure[i] = -1
# 修改各图片中的索引
for a in range(len(correspond_struct_idx)):
for b in range(len(correspond_struct_idx[a])):
if correspond_struct_idx[a][b] != -1:
if structure[int(correspond_struct_idx[a][b])][0] == -1:
correspond_struct_idx[a][b] = -1
else:
correspond_struct_idx[a][b] -= (np.sum(
structure[:int(correspond_struct_idx[a][b])] == -1) /
3)
# 删除那些为空的点
i = 0
while i < len(structure):
if structure[i][0] == -1:
structure = np.delete(structure, i, 0)
colors = np.delete(colors, i, 0)
i -= 1
i += 1
# ----- 计算重投影误差: 是否考虑畸变
reproj_err(rotations, motions, K, correspond_struct_idx, keypoints_for_all,
structure)
## ----------
print('BA优化...')
motions = np.array(motions)
rotations = np.array(rotations)
structure = bundle_adjustment(structure, correspond_struct_idx, motions,
rotations, keypoints_for_all, K)
## ----------
# ----- 计算重投影误差: 是否考虑畸变
reproj_err(rotations, motions, K, correspond_struct_idx, keypoints_for_all,
structure)
# 保存Bundle.rd.out
print("点云已生成，正在保存.out文件")
# 旋转向量转化为旋转矩阵
Rotations = np.empty((len(rotations), 3, 3))
for i in range(len(rotations)):
R, _ = cv2.Rodrigues(rotations[i])
Rotations[i] = R
save_bundle_rd_out(structure, K, Rotations, motions, colors,
correspond_struct_idx, keypoints_for_all)
np.save(image_dir + 'Structure', structure)
np.save(image_dir + 'Colors', colors)
np.save(image_dir + 'Projections', projections)
dist_coefs[1] = -0.23243439
dist_coefs[2] = -0.02683919
dist_coefs[3] = -0.00194744
dist_coefs[4] = 0.24594352
#for i in [2,3,4,6,8,10,12,'6-rot']:
for i in [8]:
src = 'image/' + str(i) + 'ft.jpg'
out = "out/" + str(i) + '.jpg'
print("locating", i)
#locate_xyz(src, (known_dist, w), out)
objects, image = identify_objects(src)
objs = []
imgs = []
for obj in objects:
code, size, rect = obj
center = rect_center(rect)
point = object_points[str(code)]
imgs.append([center])
objs.append([point])
objs = np.array(objs, dtype=np.float)
imgs = np.array(imgs, dtype=np.float)
solve = cv2.solvePnPRansac(objs, imgs, camera_matrix, dist_coefs)
print(solve)
break
def calculate_odometry(im1, ref_graph, ref_cloud_dict, ref_orb_dict, rgbd,
step):
rgb_im, depth_im = rgbd
cloud_dict, orb_dict = get_clustered_point_cloud(np.array(rgb_im),
np.array(depth_im))
graph = construct_graph(cloud_dict)
# This section is for converting ORB Match --> Object Match
match_dict = {}
kp1, des1, assign1 = ref_orb_dict["kp"], ref_orb_dict["des"], ref_orb_dict[
'assign']
kp2, des2, assign2 = orb_dict["kp"], orb_dict["des"], orb_dict['assign']
# BFMatcher with default params
bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
matches = bf.match(des1, des2)
matches = sorted(matches, key=lambda x: x.distance)
for i, m in enumerate(matches):
idx1, idx2 = m.queryIdx, m.trainIdx
l1, l2 = assign1[idx1], assign2[idx2]
if l1 == -1 or l2 == -1:
continue
match_str = str(l1) + '-' + str(l2)
if match_str not in match_dict:
match_dict[match_str] = 1
else:
match_dict[match_str] += 1
# print(match_dict)
res = {}
s1, s2 = set(), set()
for match_str, votes in sorted(match_dict.items(), key=lambda x: -x[1]):
arr = match_str.split("-")
idx1, idx2 = int(arr[0]), int(arr[1])
if votes >= min_votes and idx1 not in s1 and idx2 not in s2:
res[idx1] = idx2
s1.add(idx1)
s2.add(idx2)
#print(res)
Rs, Ts = [], []
K = cv2.UMat(
np.array([[525.0, 0.0, 319.5], [0.0, 525.0, 239.5], [0.0, 0.0, 1.0]],
dtype=np.float32))
for o_idx1, o_idx2 in res.items():
#print(o_idx1, o_idx2)
kp1, kp2 = ref_cloud_dict[o_idx1]['kp_arr'], cloud_dict[o_idx2][
'kp_arr']
des1, des2 = ref_cloud_dict[o_idx1]['des_arr'], cloud_dict[o_idx2][
'des_arr']
des1, des2 = cv2.UMat(np.array(des1, dtype=np.uint8)), cv2.UMat(
np.array(des2, dtype=np.uint8))
tmp_matches = bf.match(des1, des2)
tmp_matches = sorted(tmp_matches, key=lambda x: x.distance)
num = len(tmp_matches)
# im3 = cv2.drawMatches(cv2.UMat(np.array(im1)),kp1,cv2.UMat(np.array(rgb_im)),kp2,tmp_matches[:num],outImg=None)
# plt.imshow(im3.get())
# plt.savefig('step_'+str(step)+'_'+str(o_idx1)+"-"+str(o_idx2)+".jpg")
# plt.show()
xyz_arr, uv_arr = [], []
K = np.array([[525.0, 0.0, 319.5], [0.0, 525.0, 239.5],
[0.0, 0.0, 1.0]])
for i in range(num):
m = tmp_matches[i]
f_idx1, f_idx2 = m.queryIdx, m.trainIdx
xyz_arr.append(ref_cloud_dict[o_idx1]['xyz_arr'][f_idx1])
uv_arr.append(np.array(cloud_dict[o_idx2]['uv_arr'][f_idx2]))
#print('number of matches:', np.array(xyz_arr).shape[0])
xyz_arr, uv_arr = cv2.UMat(np.array(xyz_arr,
dtype=np.float32)), cv2.UMat(
np.array(uv_arr,
dtype=np.float32))
rvec, tvec = cv2.UMat(np.array([0.0, 0.0, 0.0
])), cv2.UMat(np.array([0.0, 0.0,
0.0]))
flag, rvec, tvec, inliers = cv2.solvePnPRansac(xyz_arr, uv_arr, K,
None, rvec, tvec, True)
# print(str(o_idx1)+"-"+str(o_idx2)+'rotation:',rvec.get())
# print(str(o_idx1)+"-"+str(o_idx2)+'translation:',tvec.get())
#print("number of inliers:",len(inliers.get()))
Rs.append(rvec.get())
Ts.append(tvec.get())
RMs = []
for rvec in Rs:
r = R.from_rotvec(rvec.reshape(3, ))
RMs.append(r.as_euler('xyz'))
RMs = np.array(RMs).reshape(-1, 3)
Ts = np.array(Ts).reshape(-1, 3)
R_res = rotation_ransac(RMs)
t_res = trans_ransac(Ts)
# print('step:'+str(step), R_res, t_res)
return R_res, t_res
def main_loop():
while True:
rv, fr = cap.read()
fr_lab = cv2.cvtColor(fr, cv2.COLOR_BGR2LAB)
channels = cv2.split(fr_lab.astype('int16'))
greenscale = np.zeros(fr.shape[:-1], 'int16')
for tr, ch, fac in zip(targetColor, channels, lab_factors):
greenscale += np.absolute(ch - tr) // fac
#print(greenscale.max())
greenscale = 255 - np.clip((greenscale), 0, 255).astype('uint8')
cv2.imshow('greenscale', greenscale)
if not rv: break
pipeline.process(fr)
op = pipeline.hsv_threshold_output
o8 = op.astype('uint8') * 128
corners = cv2.cornerHarris(np.float32(greenscale), 4, 3, 0.03)
#print(corners.min(), corners.max())
#cm = corners + (-corners.min() + 1)
#print(cm.min(), cm.max())
#lcorn = np.log(cm)
#print(lcorn.min(), lcorn.max())
#cv2.imshow('corners', lcorn / 16)
kernel = np.ones((5, 5), np.uint8)
eroded_hsv_1 = cv2.dilate(cv2.erode(op, kernel, iterations=1),
kernel,
iterations=1) #.astype(np.bool_)
eroded_hsv = cv2.dilate(eroded_hsv_1, kernel, iterations=2)
contours, hier = cv2.findContours(eroded_hsv, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
biggest_c = itertools.islice(
sorted(contours, key=cv2.contourArea, reverse=True), 2)
zer = np.zeros(eroded_hsv.shape, np.uint8)
for i in biggest_c:
cv2.fillPoly(zer, pts=[i], color=255)
cv2.drawContours(fr, i, -1, (255, 255, 255), 1)
cactual = (corners > 0.0085 * corners.max()) & zer.astype(
np.bool_) #eroded_hsv
ret, labels, stats, centroids = cv2.connectedComponentsWithStats(
cactual.astype('uint8'))
#fr[eroded_hsv_1.astype(np.bool_),2] = 255
fr[cactual] = (0, 0, 255)
fc = []
for x, y in centroids[1:]:
#fr[int(y), int(x)] = (0, 255, 0)
ix, iy = int(x), int(y)
cc = eroded_hsv_1[iy - 10:iy + 10,
ix - 10:ix + 10].astype(np.bool_).sum()
if 15 < cc < 160:
cv2.putText(fr, str(cc), (ix, iy), cv2.FONT_HERSHEY_SIMPLEX,
0.5, (255, 255, 255), 1, cv2.LINE_AA)
#cv2.circle(fr, (int(x), int(y)), 2, (0, 255, 0), -1)
fc.append((x, y))
else:
pass
#cv2.circle(fr, (int(x), int(y)), 2, (0, 0, 255), -1)
c_exact = []
inliers = None
if fc:
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER,
100, 0.001)
c_exact = cv2.cornerSubPix(greenscale, np.float32(fc), (5, 5),
(-1, -1), criteria)
#for x, y in c_exact:
# cv2.circle(fr, (int(x), int(y)), 1, (255, 0, 0), -1)
#for i, (x, y, z) in enumerate(world_pts):
# cv2.putText(fr,str(i),(int(x*5),int(y*5)+50), cv2.FONT_HERSHEY_SIMPLEX, 1,(255,255,255),1,cv2.LINE_AA)
if len(c_exact) == 8:
h = list(sorted(c_exact, key=lambda x: x[0]))
left = h[:4]
right = h[4:]
pts = []
for rect in left, right:
centerx, centery = sum(x[0]
for x in rect) / len(rect), sum(
x[1] for x in rect) / len(rect)
def key(r):
x, y = r
dx, dy = x - centerx, y - centery
return math.atan2(-dy, dx) % (2 * math.pi)
pts += sorted(rect, key=key)
for i, (x, y) in enumerate(pts):
cv2.putText(fr, str(i), (int(x), int(y)),
cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255),
1, cv2.LINE_AA)
pts = np.float32(pts)
#print(pts)
retval2, rvecs, tvecs, inliers = cv2.solvePnPRansac(
world_pts, pts, cam_mtrx,
distorts) #, flags=cv2.SOLVEPNP_EPNP)
cv2.putText(
fr, 'I: {}'.format(
len(inliers) if inliers is not None else 'X'),
(550, 50), cv2.FONT_HERSHEY_SIMPLEX, 1,
(255, 255,
255) if inliers is not None and len(inliers) == 8 else
(0, 0, 255), 1, cv2.LINE_AA)
#retval2, rvecs, tvecs = cv2.solvePnP(world_pts, pts, cam_mtrx, distorts)
#print(inliers)
#print(rvecs, tvecs)
if inliers is not None:
centerp = np.float32([14.627 / 2, -5.325 / 2, 0])
#global g_rot, g_pos
#g_rot = rvecs
#g_pos = tvecs
rot_history.append(rvecs)
pos_history.append(tvecs)
if len(rot_history) > 5:
del rot_history[0]
del pos_history[0]
#ax2 = axis + centerp
#print(ax2)
#ax2 = np.insert(ax2, 0, centerp, axis=0)#np.float32([centerp]) + ax2
#print(ax2)
#imgpts, jac = cv2.projectPoints(ax2, rvecs, tvecs, cam_mtrx, distorts)
#fr =draw(fr, imgpts[0], imgpts[1:])
qb = cube + centerp
imgpts, jac = cv2.projectPoints(qb, rvecs, tvecs, cam_mtrx,
distorts)
fr = draw_cube(fr, None, imgpts)
pts2, jac = cv2.projectPoints(world_pts, rvecs, tvecs,
cam_mtrx, distorts)
for x, y in np.int32(pts2).reshape(-1, 2):
cv2.circle(fr, (x, y), 2, (255, 255, 255), -1)
else:
cv2.putText(fr, 'C: {}'.format(len(c_exact)), (550, 25),
cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1,
cv2.LINE_AA)
#pipeline.hsv_threshold_output
#print(corners)
if len(rot_history) > 0 and inliers is None:
del rot_history[0]
del pos_history[0]
cv2.imshow('f1', fr)
cv2.imshow('dsst', op)
cv2.imshow('bc', zer)
if cv2.waitKey(1) & 0xff == 27: break
cv2.destroyAllWindows()
def estimate_pose(self):
search_size = (5, 4)
axis = np.float32([[3, 0, 0], [0, 3, 0], [0, 0, 3]]).reshape(-1, 3)
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
objp = np.zeros((search_size[0] * search_size[1], 3), np.float32)
objp[:, :2] = np.mgrid[0:search_size[0], 0:search_size[1]].T.reshape(-1, 2)
cap = cv2.VideoCapture('../videos/right_sd_test2.avi')
trans = []
rot = []
n = 2400
for i in range(n):
ret, frame = cap.read()
grey = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(grey, search_size, None)
if ret:
cv2.cornerSubPix(grey, corners, (11, 11), (-1, -1), criteria)
rvecs, tvecs, inliers = cv2.solvePnPRansac(objp, corners, self.cam_matrix, self.dist_matrix)
rot.append(self.find_euler_angles(rvecs))
trans.append(self.find_pos(tvecs))
else:
print 'Corners not found in this frame'
rot.append([0, 0, 0])
trans.append([0, 0, 0])
#cv2.imshow('frame', frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
cap.release()
cv2.destroyAllWindows()
# Condition zero data points
for i in range(1, n-1):
if rot[i] == [0, 0, 0] and rot[i + 1] != [0, 0, 0]:
rot[i] = [(rot[i-1][0] + rot[i + 1][0]) / 2, (rot[i-1][1] + rot[i + 1][1]) / 2, (rot[i-1][2] + rot[i + 1][2]) / 2]
trans[i] = [(trans[i-1][0] + trans[i + 1][0]) / 2, (trans[i-1][1] + trans[i + 1][1]) / 2, (trans[i-1][2] + trans[i + 1][2]) / 2]
elif rot[i] == [0, 0, 0] and rot[i + 1] == [0, 0, 0]:
for j in range(i, n-1):
if rot[j] != [0, 0, 0]:
last_index = j
break
if 'last_index' in locals():
for j in range(i, last_index):
rot[j] = [rot[j - 1][0] + ((rot[last_index][0] - rot[i - 1][0]) / (last_index - i + 1)),
rot[j - 1][1] + ((rot[last_index][1] - rot[i - 1][1]) / (last_index - i + 1)),
rot[j - 1][2] + ((rot[last_index][2] - rot[i - 1][2]) / (last_index - i + 1))]
trans[j] = [trans[j - 1][0] + ((trans[last_index][0] - trans[i - 1][0]) / (last_index - i + 1)),
trans[j - 1][1] + ((trans[last_index][1] - trans[i - 1][1]) / (last_index - i + 1)),
trans[j - 1][2] + ((trans[last_index][2] - trans[i - 1][2]) / (last_index - i + 1))]
trans = np.asarray(trans)
rot = np.asarray(rot)
trans[:, 0] = [trans[:, 0][i]*-10 + 0 for i in range(0, len(trans[:, 0]))]
trans[:, 1] = [trans[:, 1][i]*-10 + 0 for i in range(0, len(trans[:, 1]))]
trans[:, 2] = [trans[:, 2][i]*-10 + 0 for i in range(0, len(trans[:, 2]))]
rot[:, 0] = [rot[:, 0][i] - 0 for i in range(0, len(rot[:, 0]))]
rot[:, 1] = [rot[:, 1][i]*-1 for i in range(0, len(rot[:, 1]))]
rot[:, 2] = [rot[:, 2][i] - 90 if rot[:, 2][i] > 0 else rot[:, 2][i] + 90 for i in range(0, len(rot[:, 2]))]
rot[:, 2] = [rot[:, 2][i] - 0 for i in range(0, len(rot[:, 2]))]
temp_rot = np.zeros(rot.shape)
temp_trans = np.zeros(trans.shape)
# Make cam coordinate system coincide with Vicon coordinate system
temp_trans[:, 0] = trans[:, 0]
temp_trans[:, 1] = trans[:, 2]#*1.5
temp_trans[:, 2] = trans[:, 1]
temp_rot[:, 0] = rot[:, 2]
temp_rot[:, 1] = rot[:, 1]
temp_rot[:, 2] = rot[:, 0]
return temp_trans.T, temp_rot.T
def track_and_calc_colors(camera_parameters: CameraParameters,
corner_storage: CornerStorage,
frame_sequence_path: str,
known_view_1: Optional[Tuple[int, Pose]] = None,
known_view_2: Optional[Tuple[int, Pose]] = None) \
-> Tuple[List[Pose], PointCloud]:
rgb_sequence = frameseq.read_rgb_f32(frame_sequence_path)
frames_num = len(rgb_sequence)
intrinsic_mat = to_opencv_camera_mat3x3(camera_parameters,
rgb_sequence[0].shape[0])
if known_view_1 is None or known_view_2 is None:
known_view_1, known_view_2 = select_frames(rgb_sequence,
corner_storage,
intrinsic_mat)
if known_view_2[0] == -1:
print("Failed to find good starting frames")
exit(0)
correspondences = build_correspondences(corner_storage[known_view_1[0]],
corner_storage[known_view_2[0]])
view_mat1 = pose_to_view_mat3x4(known_view_1[1])
view_mat2 = pose_to_view_mat3x4(known_view_2[1])
points, ids, _ = triangulate_correspondences(correspondences, view_mat1,
view_mat2, intrinsic_mat,
TRIANG_PARAMS)
if len(ids) < 8:
print("Bad frames: too few correspondences!")
exit(0)
view_mats, point_cloud_builder = [None] * frames_num, PointCloudBuilder(
ids, points)
view_mats[known_view_1[0]] = view_mat1
view_mats[known_view_2[0]] = view_mat2
updated = True
pass_num = 0
while updated:
updated = False
pass_num += 1
for i in range(frames_num):
if view_mats[i] is not None:
continue
corners = corner_storage[i]
_, ind1, ind2 = np.intersect1d(point_cloud_builder.ids,
corners.ids.flatten(),
return_indices=True)
try:
_, rvec, tvec, inliers = cv2.solvePnPRansac(
point_cloud_builder.points[ind1],
corners.points[ind2],
intrinsic_mat,
distCoeffs=None)
if inliers is None:
print(f"Pass {pass_num}, frame {i}. No inliers!")
continue
print(
f"Pass {pass_num}, frame {i}. Number of inliers == {len(inliers)}"
)
view_mats[i] = rodrigues_and_translation_to_view_mat3x4(
rvec, tvec)
except Exception:
continue
updated = True
cloud_changed = add_points(point_cloud_builder, corner_storage, i,
view_mats, intrinsic_mat)
if cloud_changed:
print(f"Size of point cloud == {len(point_cloud_builder.ids)}")
first_not_none = next(item for item in view_mats if item is not None)
if view_mats[0] is None:
view_mats[0] = first_not_none
for i in range(frames_num):
if view_mats[i] is None:
view_mats[i] = view_mats[i - 1]
calc_point_cloud_colors(point_cloud_builder, rgb_sequence, view_mats,
intrinsic_mat, corner_storage, 5.0)
point_cloud = point_cloud_builder.build_point_cloud()
poses = list(map(view_mat3x4_to_pose, view_mats))
return poses, point_cloud
def alg_1(img):
image = cv2.imread(img)
image = cv2.cvtColor(image,
cv2.COLOR_BGR2GRAY) # convert image to grayscale
size = image.shape
#templateModel = o3d.io.read_point_cloud("data/template.ply")
pcd = o3d.io.read_point_cloud("Data/face_mesh_000306.ply")
# estimate radius for rolling ball
distances = pcd.compute_nearest_neighbor_distance()
avg_dist = np.mean(distances)
radius = 1.5 * avg_dist
average_face_mesh = o3d.io.read_triangle_mesh('Data/face_mesh_000306.ply')
# 2D image points
image_points = optcal_flow.optical_flow(
img, image_renderer.img_renderer(average_face_mesh))
# 3D object points (model point)
def process_template(templatePoints, templateCoordinates):
"""This function recentres the point cloud about the min value in each axis.
Also applies the same transformations to the point cloud fiducial points.
:param templatePoints: object that stores information about the point cloud
:type templatePoints: open3d Point Cloud object
:param templateCoordinates:
:type templateCoordinates:
"""
# get the min value in each axis
p1 = min(templatePoints[:, 0])
p2 = min(templatePoints[:, 1])
p3 = min(templatePoints[:, 2])
# subtract the mean
templatePoints[:, 0] -= p1
templatePoints[:, 1] -= p2
templatePoints[:, 2] -= p3
templateCoordinates[:, 0] -= p1
templateCoordinates[:, 1] -= p2
templateCoordinates[:, 2] -= p3
# divide by two. Why? -> maybe to reduce the range of values??
templatePoints[:, 0] /= 2
templatePoints[:, 1] /= 2
templatePoints[:, 2] /= 2
templateCoordinates[:, 0] /= 2
templateCoordinates[:, 1] /= 2
templateCoordinates[:, 2] /= 2
return templatePoints, templateCoordinates
templateCoordinates = np.array([
[34.9, 2.458, 1230], # right eye right
[37, -31.89, 1236], # right eye left
[45.4, -66.51, 1248], # left eye right
[41.24, -94.92, 1267], # left eye left
[82.49, -29.01, 1227], # nose right
[80.5, -53.6, 1212], # nose tip
[79.03, -71.2, 1240], # nose left
[105.5, -23.84, 1228], # mouth right
[111.5, -72.14, 1251] # mouth left
])
#templateModel = o3d.io.read_point_cloud("data/template.ply")
ptCloud = o3d.io.read_point_cloud(
"/Users/angusharrington/Documents/AEK_total_moving_faces/3D/perfect_dubbing_v1/3D_flow/data/template.ply"
)
templatePoints = UpsamplePtCloud(ptCloud)
templatePoints, templateCoordinates = process_template(
templatePoints, templateCoordinates)
model_points = o3d.find3dpoints(templateCoordinates)
# Camera
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))
iterations_count = 50
reprojection_error = 0.1
min_inliers_count = 400
'''Python: cv2.solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs[, rvec[, tvec[, useExtrinsicGuess[,
flags]]]]) → retval, rvec, tvec'''
# Solve PnP Ransac(but there are only nine points so is this necessary?)
(rotation_vector1, translation_vector1,
inlier_indices) = cv2.solvePnPRansac(model_points,
image_points,
camera_matrix,
dist_coeffs,
iterations_count,
reprojection_error,
min_inliers_count,
flags=cv2.CV_ITERATIVE)
inliers_object = [model_points[i] for i in inlier_indices]
inliers_image = [image_points[i] for i in inlier_indices]
# Solve Pnp on all inliers
(success, rotation_vector,
translation_vector) = cv2.solvePnP(inliers_object,
inliers_image,
camera_matrix,
dist_coeffs,
flags=cv2.CV_ITERATIVE)
return print(success, rotation_vector, translation_vector)
def solve_pnp_ransac(
canonical_points,
projections,
camera_K,
method=cv2.SOLVEPNP_EPNP,
inlier_thresh_px=5.0, # this is the threshold for each point to be considered an inlier
dist_coeffs=np.array([]),
):
n_canonial_points = len(canonical_points)
n_projections = len(projections)
assert (
n_canonial_points == n_projections
), "Expected canonical_points and projections to have the same length, but they are length {} and {}.".format(
n_canonial_points, n_projections
)
# Process points to remove any NaNs
canonical_points_proc = []
projections_proc = []
for canon_pt, proj in zip(canonical_points, projections):
if (
canon_pt is None
or len(canon_pt) == 0
or canon_pt[0] is None
or canon_pt[1] is None
or proj is None
or len(proj) == 0
or proj[0] is None
or proj[1] is None
):
continue
canonical_points_proc.append(canon_pt)
projections_proc.append(proj)
# Return if no valid points
if len(canonical_points_proc) == 0:
return False, None, None, None
canonical_points_proc = np.array(canonical_points_proc)
projections_proc = np.array(projections_proc)
# Use cv2's PNP solver
try:
pnp_retval, rvec, tvec, inliers = cv2.solvePnPRansac(
canonical_points_proc.reshape(canonical_points_proc.shape[0], 1, 3),
projections_proc.reshape(projections_proc.shape[0], 1, 2),
camera_K,
dist_coeffs,
reprojectionError=inlier_thresh_px,
flags=method,
)
translation = tvec[:, 0]
quaternion = convert_rvec_to_quaternion(rvec[:, 0])
except:
pnp_retval = False
translation = None
quaternion = None
inliers = None
return pnp_retval, translation, quaternion, inliers
# Calculate current pointcloud
current_pointcloud, _, kp2_updated_current = calculate_pointcloud(previous_left_image, previous_right_image, current_left_image, depth_image)
# Check to see that the pointcloud is long enough for PNPRansac
if len(current_pointcloud) < 4:
previous_left_image = current_left_image
previous_right_image = current_right_image
frame_counter+=1
continue
# Calculate rotation and translation
_, R_v, t, _ = cv2.solvePnPRansac(current_pointcloud, kp2_updated_current, camera_matrix,None)
R, _ = cv2.Rodrigues(R_v)
# Deal with anomaly translational values
if t[0] > 1:
t[0] = 0.25
if t[2] > 1:
t[2] = 0.25
# Update total translation and rotation
R_f = R.dot(R_f)
t_f = t_f + R_f.dot(t)
break
# user input to find another chessboard pattern
if cv2.waitKey(1) & 0xFF == ord('n'):
find = True
if find:
find = False
ret, corners = cv2.findChessboardCorners(gray, (8, 6), None)
if ret == True:
corners2 = cv2.cornerSubPix(gray, corners, (3, 3), (-1, -1),
criteria)
# Find the rotation and translation vectors.
ret, rvecs, tvecs, inliers = cv2.solvePnPRansac(
objp, corners2, mtx, dist)
print("rotation")
print("x", rvecs[0])
print("y", rvecs[1])
print("z", rvecs[2])
print()
print("translation")
print("yaw", tvecs[0])
print("pitch", tvecs[1])
print("roll", tvecs[2])
print()
# project 3D points to image plane
imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)
img = draw(img, corners2, imgpts)
# %% determino la pose a prior
# saco una pose inicial con opencv
# saco condiciones iniciales para los parametros uso opencv y heuristica
#reload(cl)
if model == modelos[3]:
# saco rVecs, tVecs de la galera
# tvecIni = camPrior
# rvecIni = np.array([np.pi, 0, 0]) # camara apuntando para abajo
Xext0 = camPrior.copy()
rvecIni, tvecIni = bl.flat2ext(Xext0)
else:
ret, rvecIni, tvecIni, inliers = cv2.solvePnPRansac(calibPts, xIm,
cameraMatrix, distCoeffs)
Xext0 = bl.ext2flat(rvecIni, tvecIni)
xpr, ypr, c = cl.inverse(xIm, rvecIni, tvecIni, cameraMatrix, distCoeffs, model=model)
plt.plot(xpr, ypr, '*')
# %%
std = 1.0
# output file
#extrinsicParamsOutFile = extrinsicFolder + camera + model + "intrinsicParamsAP"
#extrinsicParamsOutFile = extrinsicParamsOutFile + str(std) + ".npy"
Ci = np.repeat([ std**2 * np.eye(2)],n*m, axis=0).reshape(n,m,2,2)
params = dict()
try:
while True:
#img = cv2.imread(fname)
ret, img = cap.read()
if (not ret):
break
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, (4, 3), 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, None, None)
rvecs, tvecs, inliers = cv2.solvePnPRansac(objp, corners, None,
None)
# project 3D points to image plane
imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)
img = draw(img, corners2, imgpts)
cv2.imshow('img', img)
cv2.waitKey(10)
#k = cv2.waitKey(0) & 0xff
#if k == 's':
# cv2.imwrite(fname[:6]+'.png', img)
finally:
cap.release()
cv2.destroyAllWindows()
def alg_1(img, avg_face_mesh):
light_est, shape_est = InitialLightingAndShapeEstimation(img)
size = img.shape()
templateModel = o3d.io.read_point_cloud("Data/face_mesh_000306.ply")
pcd = o3d.io.read_point_cloud(templateModel)
radii =
avg_face_mesh = o3d.create_from_point_cloud_ball_pivoting(pcd, radii)
# 2D image points
image_points = optcal_flow.optical_flow(img, img_renderer(img, avg_face_mesh, light_est))
# 3D object points (model point)
def process_template(templatePoints, templateCoordinates):
"""This function recentres the point cloud about the min value in each axis.
Also applies the same transformations to the point cloud fiducial points.
:param templatePoints: object that stores information about the point cloud
:type templatePoints: open3d Point Cloud object
:param templateCoordinates:
:type templateCoordinates:
"""
# get the min value in each axis
p1 = min(templatePoints[:, 0])
p2 = min(templatePoints[:, 1])
p3 = min(templatePoints[:, 2])
# subtract the mean
templatePoints[:, 0] -= p1
templatePoints[:, 1] -= p2
templatePoints[:, 2] -= p3
templateCoordinates[:, 0] -= p1
templateCoordinates[:, 1] -= p2
templateCoordinates[:, 2] -= p3
# divide by two. Why? -> maybe to reduce the range of values??
templatePoints[:, 0] /= 2
templatePoints[:, 1] /= 2
templatePoints[:, 2] /= 2
templateCoordinates[:, 0] /= 2
templateCoordinates[:, 1] /= 2
templateCoordinates[:, 2] /= 2
return templatePoints, templateCoordinates
templateCoordinates = np.array([[35.53, -428.7, 1152],
[39.07, -456.8, 1148],
[42.61, -495, 1148],
[39.77, -528, 1154],
[85.22, -456.7, 1138],
[85.77, -475.1, 1122],
[88.47, -490.1, 1136],
[107.7, -448.1, 1147],
[107.8, -499.6, 1144]])
templateModel = o3d.io.read_point_cloud("Data/face_mesh_000306.ply")
ptCloud = o3d.io.read_point_cloud(templateModel)
templatePoints = UpsamplePtCloud(ptCloud)
templatePoints, templateCoordinates = process_template(templatePoints, templateCoordinates)
model_points = o3d.find3dpoints(templateCoordinates)
# Camera
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))
iterations_count = 50
reprojection_error = 0.1
min_inliers_count = 400
'''Python: cv2.solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs[, rvec[, tvec[, useExtrinsicGuess[,
flags]]]]) → retval, rvec, tvec'''
# Solve PnP Ransac(but there are only nine points so is this necessary?)
(rotation_vector1, translation_vector1, inlier_indices) = cv2.solvePnPRansac(model_points, image_points, camera_matrix, dist_coeffs,
iterations_count, reprojection_error, min_inliers_count, flags=cv2.CV_ITERATIVE)
inliers_object =
inliers_image =
# Solve Pnp on all inliers
(success, rotation_vector, translation_vector) = cv2.solvePnP(inliers_object, inliers_image, camera_matrix, dist_coeffs, flags=cv2.CV_ITERATIVE)
criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001)
objp = np.zeros((num_corners.col * num_corners.row, 3), np.float32)
objp[:, :2] = np.mgrid[0:num_corners.row,
0:num_corners.col].T.reshape(-1, 2)
axis = np.float32([[3, 0, 0], [0, 3, 0], [0, 0, -3]]).reshape(-1, 3)
if corners_found:
refined_corners = cv.cornerSubPix(gray_image, raw_corners, (11, 11),
(-1, -1), criteria)
length = abs(refined_corners[0][0][1] - refined_corners[3][0][1])
#focal_lenth = (length[0]*known_distance)/known_width
distance = distance_to_camera(known_length, focal_length, length)
# Find the rotation and translation vectors.
try:
ret, rvecs, tvecs, inliers = cv.solvePnPRansac(
objp, refined_corners, mtx, dist) #try Ransac
except:
print("error")
continue
#r = R.from_rotvec(3,rvecs)
# Convert 3x1 rotation vector to rotation matrix for further computation
rotation_matrix, jacobian = cv.Rodrigues(rvecs)
tvecs_new = -np.matrix(rotation_matrix).T * np.matrix(tvecs)
# Projection Matrix
pmat = np.hstack((rotation_matrix, tvecs)) # [R|t]
roll, pitch, yaw = cv.decomposeProjectionMatrix(pmat)[-1]
x_distance = math.cos(roll * math.pi / 180) * distance
y_distance = abs(math.sin(roll * math.pi / 180) * distance)
xy_pair = (x_distance, y_distance)
img = "Marker_sample.jpg"
pts, flag = Points(img)
if flag == '1':
imgp = Perspective(pts, img)
elif flag == '2':
imgp = crop(pts, img)
elif flag == '-1':
quit()
print "Image points\n\n", imgp
mtx = np.load('matrix.npy')
dist = np.load('distortion.npy')
rvec, tvec, inliers = cv2.solvePnPRansac(objp, imgp, mtx, dist)
print "Rvec\n", rvec
print "\nTvec", tvec
dst, jacobian = cv2.Rodrigues(rvec)
x = tvec[0][0]
y = tvec[2][0]
t = (math.asin(-dst[0][2]))
print "X", x, "Y", y, "Angle", t
print "90-t", (math.pi / 2) - t
Rx = y * (math.cos((math.pi / 2) - t))
Ry = y * (math.sin((math.pi / 2) - t))
print "rx", Rx, "ry", Ry
def main(self):
with np.load(self.__filename) as X:
ret, mtx, dist, _, _ = [X[i] for i in ('ret', 'mtx', 'dist', 'rvecs', 'tvecs')]
termination = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
row = self.__row
col = self.__col
height = self.__height
objp = np.zeros((row * col, 3), np.float32)
objp[:, :2] = np.mgrid[0:row, 0:col].T.reshape(-1, 2)
axis = np.float32([[0, 0, 0], [0, col - 1, 0], [row - 1, col - 1, 0], [row - 1, 0, 0], [0, 0, -height + 1],
[0, col - 1, -height + 1], [row - 1, col - 1, -height + 1], [row - 1, 0, -height + 1]])
try:
while True:
if self.__cam_type == 'USB':
ret, self.__img_raw_cam = self.cap.read()
if not ret:
print ("video reading error")
break
elif self.__cam_type == 'REALSENSE':
# Wait for a coherent pair of frames: depth and color
frames = self.pipeline.wait_for_frames()
color_frame = frames.get_color_frame()
# Convert images to numpy arrays
self.__img_raw_cam = np.asanyarray(color_frame.get_data())
if self.__img_raw_cam != []:
img = self.__img_raw_cam
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# Find the chess board corners
ret, corners = cv2.findChessboardCorners(gray, (self.__row, self.__col), None)
if ret == True:
cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), termination)
_, rvecs, tvecs, inliers = cv2.solvePnPRansac(objp, corners, mtx, dist)
# print rvecs[0], rvecs[1], rvecs[2]
# Rc1m = np.array(cv2.Rodrigues(rvecs)[0]) # 3x3 rotation matrix
# tc1m = np.array(tvecs) # translational vector
# Tc1m = np.vstack((np.hstack((Rc1m, tc1m)), [0, 0, 0, 1])) # 4x4 homogeneous transformation matrix
# print Tc1m
# Put text presenting the distance
font = cv2.FONT_HERSHEY_SIMPLEX
pos_str = "%0.1f, %0.1f, %0.1f" % (tvecs[0], tvecs[1], tvecs[2])
rot_str = "%0.1f, %0.1f, %0.1f" % (rvecs[0]*180/3.14, rvecs[1]*180/3.14, rvecs[2]*180/3.14)
cv2.putText(img, pos_str, (20,50), font, 1, (0, 255, 0), 3)
cv2.putText(img, rot_str, (20,80), font, 1, (0, 255, 0), 3)
imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)
img = self.drawCube(img, imgpts)
cv2.imshow("AR", img)
key = cv2.waitKey(1) & 0xFF
if key == ord('q'): # ENTER
break
finally:
if self.__cam_type == 'USB':
self.cap.release()
cv2.destroyAllWindows()
elif self.__cam_type == 'REALSENSE':
# Stop streaming
self.pipeline.stop()
def process(self, msg, found_cb):
img = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
#img = imutils.resize(img, width = int(img.shape[1] * 1))
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
img2 = gray
img3 = gray
orb = cv2.ORB_create(800)
kp = orb.detect(gray, None)
kp, des = self.orb.compute(gray, kp)
des = np.float32(des)
FLANN_INDEX_KDTREE = 0
index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5)
search_params = dict(checks=50)
flann = cv2.FlannBasedMatcher(index_params, search_params)
matches = flann.knnMatch(self.des, des, k=2)
#bf = cv2.BFMatcher()
#matches = bf.match(self.des, des)
# store all the good matches as per Lowe's ratio test.
good = []
for m, n in matches:
if m.distance < 0.75 * n.distance:
good.append(m)
if len(good) > self.min_match_count:
src_pts = np.float32([self.kp[m.queryIdx].pt
for m in good]).reshape(-1, 1, 2)
dst_pts = np.float32([kp[m.trainIdx].pt
for m in good]).reshape(-1, 1, 2)
M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 5.0)
matchesMask = mask.ravel().tolist()
h, w = self.template.shape
rect = np.float32([[0, 0], [0, h - 1], [w - 1, h - 1],
[w - 1, 0]]).reshape(-1, 1, 2)
rect3d = np.float32([[0, 0, 0], [0, h - 1, 0], [w - 1, h - 1, 0],
[w - 1, 0, 0]]).reshape(-1, 1, 3)
rect = cv2.perspectiveTransform(rect, M)
img2 = cv2.polylines(gray, [np.int32(rect)], True, 255, 3,
cv2.LINE_AA)
#dst2 = dst_pts[matchesMask].reshape(dst_pts.shape[0], 2)
#src2 = src_pts[matchesMask].reshape(dst_pts.shape[0], 2)
#src2 = np.concatenate(src2, [0], axis=1)
pnp = cv2.solvePnPRansac(rect3d, rect, self.K, self.D)
#pnp = cv2.solvePnPRansac(src2, dst2, self.K, self.D)
rvecs, tvecs, inliers = pnp[1], pnp[2], pnp[3]
# gives central position
imgpts, jac = cv2.projectPoints(self.axis + [w / 2, h / 2, 0],
rvecs, tvecs, self.K, self.D)
imgpts2, jac = cv2.projectPoints(self.axis2 + [w / 2, h / 2, 0],
rvecs, tvecs, self.K, self.D)
img3 = self.draw(img3, imgpts, imgpts2, rect)
found_cb(rvecs, tvecs / 900.0, self.name)
else:
#print "Not enough matches are found - %d/%d" % (len(good),MIN_MATCH_COUNT)
matchesMask = None
rect = np.zeros((4, 1, 2), dtype=np.int)
imgpts = np.zeros((3, 1, 2), dtype=np.int)
imgpts2 = imgpts
draw_params = dict(
matchColor=(0, 255, 0), # draw matches in green color
singlePointColor=None,
matchesMask=matchesMask, # draw only inliers
flags=2)
#img3 = cv2.cvtColor(img3, cv2.COLOR_GRAY2BGR)
#img3 = cv2.drawMatches(self.template, self.kp, gray, kp, good, None, **draw_params)
#cv2.imshow(self.name, img3)
#k = cv2.waitKey(1) & 0xff
print "ORB process9"
def main(self):
print("Main loop started\n")
# 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) ....,(row,col,0)
objp = np.zeros((self.__row * self.__col, 3), np.float32)
objp[:, :2] = np.mgrid[0:self.__row, 0:self.__col].T.reshape(-1, 2)
axis = np.float32(
[[0, 0, 0], [0, self.__col - 1, 0],
[self.__row - 1, self.__col - 1, 0], [self.__row - 1, 0, 0],
[0, 0, -self.__height + 1],
[0, self.__col - 1, -self.__height + 1],
[self.__row - 1, self.__col - 1, -self.__height + 1],
[self.__row - 1, 0, -self.__height + 1]])
# Arrays to store object points and image points from all the images.
# objpoints = [] # 3d point in real world space
# imgpoints1 = [] # 2d points in image plane.
# imgpoints2 = [] # 2d points in image plane.
# cnt = 0
mtx = [[], []]
dist = [[], []]
rvecs = [[], []]
tvecs = [[], []]
for i in range(len(self.__loadfilename)):
with np.load(self.__loadfilename[i]) as X:
_, mtx[i], dist[i], _, _ = [
X[n] for n in ('ret', 'mtx', 'dist', 'rvecs', 'tvecs')
]
try:
while True:
img1 = self.__img_raw_cam[0]
img2 = self.__img_raw_cam[1]
if img1 != [] and img2 != []:
key = cv2.waitKey(1) & 0xFF
gray1 = cv2.cvtColor(self.__img_raw_cam[0],
cv2.COLOR_BGR2GRAY)
gray2 = cv2.cvtColor(self.__img_raw_cam[1],
cv2.COLOR_BGR2GRAY)
# Find the chess board corners
ret1, corners1 = cv2.findChessboardCorners(
gray1, (self.__row, self.__col), None)
ret2, corners2 = cv2.findChessboardCorners(
gray2, (self.__row, self.__col), None)
if ret1 == True and ret2 == True:
# If found, add object points, image points (after refining them)
corners1_ = cv2.cornerSubPix(gray1, corners1, (11, 11),
(-1, -1), criteria)
corners2_ = cv2.cornerSubPix(gray2, corners2, (11, 11),
(-1, -1), criteria)
_, rvecs[0], tvecs[0], _ = cv2.solvePnPRansac(
objp, corners1_, mtx[0], dist[0])
_, rvecs[1], tvecs[1], _ = cv2.solvePnPRansac(
objp, corners2_, mtx[1], dist[1])
tc1m = np.array(tvecs[0]) * 10 # (mm)
tc2m = np.array(tvecs[1]) * 10 # (mm)
Rc1m = cv2.Rodrigues(rvecs[0])[0]
Rc2m = cv2.Rodrigues(rvecs[1])[0]
Tc1m = np.vstack((np.hstack(
(Rc1m, tc1m)), [0, 0, 0, 1]))
Tc2m = np.vstack((np.hstack(
(Rc2m, tc2m)), [0, 0, 0, 1]))
# Transformation matrix from cam 1 to cam 2
Tc1c2 = np.matrix(Tc1m) * np.linalg.inv(
np.matrix(Tc2m))
Rc1c2 = Tc1c2[0:3, 0:3]
rvecsc1c2 = np.array(cv2.Rodrigues(Rc1c2))
tvecsc1c2 = Tc1c2[0:3, 3]
# Draw cube
imgpts1, jac1 = cv2.projectPoints(
axis, rvecs[0], tvecs[0], mtx[0], dist[0])
img1 = self.drawCube(img1, imgpts1)
imgpts2, jac2 = cv2.projectPoints(
axis, rvecs[1], tvecs[1], mtx[1], dist[1])
img2 = self.drawCube(img2, imgpts2)
# Put text presenting the distance
font = cv2.FONT_HERSHEY_SIMPLEX
pos_str1 = "%0.1f, %0.1f, %0.1f" % (
tvecs[0][0], tvecs[0][1], tvecs[0][2])
rot_str1 = "%0.1f, %0.1f, %0.1f" % (
rvecs[0][0] * 180 / 3.14, rvecs[0][1] * 180 / 3.14,
rvecs[0][2] * 180 / 3.14)
pos_str3 = "%0.1f, %0.1f, %0.1f" % (
tvecsc1c2[0], tvecsc1c2[1], tvecsc1c2[2])
rot_str4 = "%0.1f, %0.1f, %0.1f" % (
rvecsc1c2[0][0] * 180 / 3.14, rvecsc1c2[0][1] *
180 / 3.14, rvecsc1c2[0][2] * 180 / 3.14)
cv2.putText(img1, pos_str1, (20, 50), font, 1,
(0, 255, 0), 3)
cv2.putText(img1, rot_str1, (20, 80), font, 1,
(0, 255, 0), 3)
cv2.putText(img1, pos_str3, (300, 430), font, 1,
(0, 255, 0), 3)
cv2.putText(img1, rot_str4, (300, 460), font, 1,
(0, 255, 0), 3)
pos_str2 = "%0.1f, %0.1f, %0.1f" % (
tvecs[1][0], tvecs[1][1], tvecs[1][2])
rot_str2 = "%0.1f, %0.1f, %0.1f" % (
rvecs[1][0] * 180 / 3.14, rvecs[1][1] * 180 / 3.14,
rvecs[1][2] * 180 / 3.14)
cv2.putText(img2, pos_str2, (20, 50), font, 1,
(0, 255, 0), 3)
cv2.putText(img2, rot_str2, (20, 80), font, 1,
(0, 255, 0), 3)
if key == ord('\r'): # ENTER
np.savez(self.__savefilename, cam_transform=Tc1c2)
print "camera transformation mtx has been saved"
if key == ord('q'): # ESD
self.__stop_flag = True
break
cv2.namedWindow('Camera 1')
cv2.moveWindow('Camera 1', 100, 50)
cv2.imshow('Camera 1', img1)
cv2.namedWindow('Camera 2')
cv2.moveWindow('Camera 2', 740, 50)
cv2.imshow('Camera 2', img2)
finally:
if self.__cam_type == 'USB':
self.cap.release()
cv2.destroyAllWindows()
elif self.__cam_type == 'REALSENSE':
# Stop streaming
self.pipeline.stop()
def computeMatches(rgb1, rgb2, depth1, depth2, CameraIntrinsicData, distCoeffs,
camera):
sift = cv2.xfeatures2d.SIFT_create()
kp1, des1 = sift.detectAndCompute(rgb1, None)
kp2, des2 = sift.detectAndCompute(rgb2, None)
print("Key points of two images: " + str(len(kp1)) + ", " + str(len(kp2)))
imgShow = None
imgShow = cv2.drawKeypoints(
rgb1, kp1, imgShow, flags=cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
cv2.imshow("keypoints_1", imgShow)
cv2.imwrite("./data/keypoints_1.png", imgShow)
cv2.waitKey(0)
imgShow = None
imgShow = cv2.drawKeypoints(
rgb2, kp2, imgShow, flags=cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
cv2.imshow("keypoints_2", imgShow)
cv2.imwrite("./data/keypoints_2.png", imgShow)
cv2.waitKey(0)
FLANN_INDEX_KDTREE = 0
index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5)
search_params = dict(checks=50) # or pass empty dictionary
matcher = cv2.FlannBasedMatcher(index_params, search_params)
matches = matcher.match(des1, des2)
print("Find total " + str(len(matches)) + " matches.")
imgMatches = None
imgMatches = cv2.drawMatches(rgb1, kp1, rgb2, kp2, matches, imgMatches)
cv2.imshow("matches", imgMatches)
cv2.imwrite("./data/matches.png", imgMatches)
cv2.waitKey(0)
goodMatches = []
minDis = 9999.0
for i in range(0, len(matches)):
if matches[i].distance < minDis:
minDis = matches[i].distance
for i in range(0, len(matches)):
if matches[i].distance < (minDis * 4):
goodMatches.append(matches[i])
print("good matches = " + str(len(goodMatches)))
imgMatches = None
imgMatches = cv2.drawMatches(rgb1, kp1, rgb2, kp2, goodMatches, imgMatches)
cv2.imshow("good_matches", imgMatches)
cv2.imwrite("./data/good_matches.png", imgMatches)
cv2.waitKey(0)
pts_obj = []
pts_img = []
for i in range(0, len(goodMatches)):
p = kp1[goodMatches[i].queryIdx].pt
d = depth1[int(p[1])][int(p[0])]
if d == 0:
pass
else:
pts_img.append(kp2[goodMatches[i].trainIdx].pt)
pd = point2dTo3d(p[0], p[1], d, camera)
pts_obj.append(pd)
pts_obj = np.array(pts_obj)
pts_img = np.array(pts_img)
cameraMatrix = np.matrix(CameraIntrinsicData)
rvec = None
tvec = None
inliers = None
retval, rvec, tvec, inliers = cv2.solvePnPRansac(pts_obj,
pts_img,
cameraMatrix,
distCoeffs,
useExtrinsicGuess=False,
iterationsCount=100,
reprojectionError=0.66)
print("inliers: " + str(len(inliers)))
print("R=" + str(rvec))
print("t=" + str(tvec))
matchesShow = []
for i in range(0, len(inliers)):
matchesShow.append(goodMatches[inliers[i][0]])
imgMatches = None
imgMatches = cv2.drawMatches(rgb1, kp1, rgb2, kp2, matchesShow, imgMatches)
cv2.imshow("inlier matches", imgMatches)
cv2.imwrite("./data/inliers.png", imgMatches)
cv2.waitKey(0)
frameWithBrightnessPoint = frame1
cv2.rectangle(frameWithBrightnessPoint, (point[0] - 5, point[1] - 5),
(point[0] + 5, point[1] + 5), (255, 0, 0), 1)
cv2.imshow('brightness1', frameWithBrightnessPoint)
point = findBrightnessPoint(frame2)
frameWithBrightnessPoint = frame2
cv2.rectangle(frameWithBrightnessPoint, (point[0] - 5, point[1] - 5),
(point[0] + 5, point[1] + 5), (255, 0, 0), 1)
cv2.imshow('brightness2', frameWithBrightnessPoint)
# If found, add object points, image points (after refining them)
if ret1 and ret2:
corners2_1 = cv2.cornerSubPix(gray1, corners1, (11, 11), (-1, -1),
criteria)
ret1, rvecs1, tvecs1, inliers1 = cv2.solvePnPRansac(
objp, corners2_1, mtx1, dist1)
corners2_2 = cv2.cornerSubPix(gray2, corners2, (11, 11), (-1, -1),
criteria)
ret2, rvecs2, tvecs2, inliers2 = cv2.solvePnPRansac(
objp, corners2_2, mtx2, dist2)
if ret1 and ret2:
rotMatrix1, _ = cv2.Rodrigues(rvecs1)
rotMatrix2, _ = cv2.Rodrigues(rvecs2)
imgpts1, _ = cv2.projectPoints(axis, rvecs1, tvecs1, mtx1, dist1)
imgpts2, _ = cv2.projectPoints(axis, rvecs2, tvecs2, mtx2, dist2)
img1 = draw(dst1, corners2_1, imgpts1)
img2 = draw(dst2, corners2_2, imgpts2)
matchesMask = mask.ravel().tolist()
h, w = img1.shape
pts = np.float32([[0, 0], [0, h - 1], [w - 1, h - 1], [w - 1, 0],
[0, h / 2], [w - 1, h / 2], [w / 2, 0],
[w / 2, 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[1][0][0]), int(dst[1][0][1])), 30,
(0, 255, 255), -1) # Bottom Left
img2 = cv2.circle(img2, (int(dst[2][0][0]), int(dst[2][0][1])), 30,
(0, 0, 255), -1) # Bottom Right
img2 = cv2.circle(img2, (int(dst[3][0][0]), int(dst[3][0][1])), 30,
(255, 255, 0), -1) # Top Right
print("c")
print(roundArray(rotationMatrixToEulerAngles(pnp[1])))
if isOk:
img = cv.flip(frame, 1) # 水平反转
img_gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
# 查找角点
is_found, corners = cv.findChessboardCorners(
img_gray, patternSize, None, cv.CALIB_CB_ADAPTIVE_THRESH +
cv.CALIB_CB_FAST_CHECK + cv.CALIB_CB_NORMALIZE_IMAGE)
if is_found:
corners2 = cv.cornerSubPix(img_gray, corners, (5, 5),
(-1, -1),
criteria) # 亚像素 优化角点位置
# cv.drawChessboardCorners(img, patternSize, corners2, True)
# 查找旋转向量和平移向量
retval, rvecs, tvecs, inliers = cv.solvePnPRansac(
obj_points, corners2, camera_matrix, dist_coeffs)
# 将3D点投影到图像平面 imgpts 为axis（3d） 在像素坐标系下的对应点
imgpts, jac = cv.projectPoints(axis, rvecs, tvecs,
camera_matrix,
dist_coeffs) # 位置估计
img = draw(img, corners2, imgpts) # 3d 重建
cv.imshow("frame", frame)
cv.imshow("img", img)
else:
continue
key = cv.waitKey(30) & 0xFF
if key == 27: # ESC 键
print("Switching between different frames ", (pts3d_11 - pts3d_11_est).sum())
# Q2. Given Cam1, 2 and 3 , can you calculate R13 ie orientation of Cam1 wrt Cam3 ?
# Ans. You need to move from Cam1 to Cam2 and then from Cam2 to Cam3. This gives us `Cam1 wrt Cam3` ie R13.
print("Estimating Camera orientations 1. -- ",
(np.matmul(R23_est1, Cam12.R) - Cam13.R).sum(), (np.matmul(R23_est2, Cam12.R) - Cam13.R).sum())
# Q3. Can you verify if the estimated R23 is correct ?
# Ans. Move from Cam2 to Cam1 , and them from Cam1 to Cam3. This gives us `Cam2 wrt Cam3` ie R23.
print("Estimating Camera orientations 2. -- ", (np.matmul(Cam13.R, Cam12.R.T) - R23_est1).sum(),
(np.matmul(Cam13.R, Cam12.R.T) - R23_est2).sum())
# From here on, we shall test the solvePnP method.
# Example-1.
val, rvec, t11_pnp_est, inliers = cv2.solvePnPRansac(pts3d_11, pts2d_11, K, None, None, None,
False, 50, 2.0, 0.99, None)
R11_pnp_est, _ = cv2.Rodrigues(rvec)
# Example-2.
val, rvec, t12_pnp_est, inliers = cv2.solvePnPRansac(pts3d_11, pts2d_12, K, None, None, None,
False, 50, 2.0, 0.99, None)
R12_pnp_est, _ = cv2.Rodrigues(rvec)
# Example-3.
val, rvec, t13_pnp_est, inliers = cv2.solvePnPRansac(pts3d_11, pts2d_13, K, None, None, None,
False, 50, 2.0, 0.99, None)
R13_pnp_est, _ = cv2.Rodrigues(rvec)
print("Solve-PnP Results ", (Cam12.R - R12_pnp_est).sum(), (Cam12.t - t12_pnp_est).sum())
def ransac(stereo_pair,
matched_fps,
stereo_cache=None,
get_pose=False,
refine=False):
assert type(stereo_pair) == sequences.PairWithStereo
if stereo_cache is not None:
raise NotImplementedError
P_C0, full_has_depth = stereo_cache[stereo_pair.imname(0)]
else:
P_C0, has_depth = pointsFromStereo(
[stereo_pair.im[0], stereo_pair.rim_0], matched_fps[0].ips_rc,
stereo_pair.K, stereo_pair.baseline)
valid_ips1 = matched_fps[1].ips_rc[:, has_depth]
if np.count_nonzero(has_depth) < 5:
print('Warning: Not enough depths!')
if not get_pose:
return np.zeros_like(has_depth).astype(bool)
else:
return np.zeros_like(has_depth).astype(bool), None
assert P_C0 is not None
assert valid_ips1 is not None
try:
if np.count_nonzero(has_depth) == 4:
# For some reason, this does not want to work...
_, rvec, tvec = cv2.solvePnP(objectPoints=np.float32(P_C0.T),
imagePoints=np.fliplr(
np.float32(valid_ips1.T)),
cameraMatrix=stereo_pair.K,
distCoeffs=None,
flags=cv2.SOLVEPNP_P3P)
inliers = np.ones(4, dtype=bool)
else:
_, rvec, tvec, inliers = cv2.solvePnPRansac(
np.float32(P_C0.T),
np.fliplr(np.float32(valid_ips1.T)),
stereo_pair.K,
distCoeffs=None,
reprojectionError=3,
flags=cv2.SOLVEPNP_P3P,
iterationsCount=3000)
except cv2.error as e:
print(P_C0)
print(valid_ips1)
raise e
if inliers is None:
print('Warning: Ransac returned None inliers!')
if not get_pose:
return np.zeros_like(has_depth).astype(bool)
else:
return np.zeros_like(has_depth).astype(bool), None
inliers = np.ravel(inliers)
if refine:
_, rvec, tvec = cv2.solvePnP(objectPoints=np.float32(P_C0.T)[inliers],
imagePoints=np.fliplr(
np.float32(valid_ips1.T)[inliers]),
cameraMatrix=stereo_pair.K,
distCoeffs=None,
rvec=rvec,
tvec=tvec,
useExtrinsicGuess=True,
flags=cv2.SOLVEPNP_ITERATIVE)
inl_indices = np.nonzero(has_depth)[0][inliers]
inlier_mask = np.bincount(inl_indices,
minlength=has_depth.size).astype(bool)
if not get_pose:
return inlier_mask
else:
T_1_0 = rpg_common_py.pose.Pose(cv2.Rodrigues(rvec)[0], tvec)
return inlier_mask, T_1_0
ret,frame = cap.read()
cv2.imshow('frame',frame)
if cv2.waitKey(1) & 0xFF == ord('p'):
print "stopped"
frame1 = frame.copy()
break
if cv2.waitKey(1) & 0xFF == ord('q'):
break
cap.release()
gray = cv2.cvtColor(frame1,cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, (9,6),None)
if ret == True:
print "ret true"
cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
#print corners
rvecs, tvecs, inliers = cv2.solvePnPRansac(objp, corners, mtx, dist)
print rvecs
print "Rmatrix"
R = cv2.Rodrigues(rvecs)[0]
print R
print "translation vector"
print tvecs
np.savez('/home/pi/ip/pose.npz',R=R,tvecs=tvecs,rvecs=rvecs)
imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)
imgpts2, jac2 = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)
print "imgpoints"
print imgpts2
#cv2.rectangle(frame1,tuple(corners[0].ravel()),tuple(corners[20].ravel()),(0,255,0),1)
corner = tuple(corners[0].ravel())
cv2.line(frame1, corner, tuple(imgpts[0].ravel()), (255,0,0), 2)
cv2.line(frame1, corner, tuple(imgpts[1].ravel()), (0,255,0), 2)
IoU = boxoverlap(box_gt, box_det)
if IoU > 0.5:
det_cls[cls_det] += 1
box3D_det = item['pose']
obj_points = np.ascontiguousarray(threeD_boxes[cls_det - 1, :, :],
dtype=np.float32) # .reshape((8, 1, 3))
est_points = np.ascontiguousarray(box3D_det, dtype=np.float32).reshape((8, 1, 2))
calib = calib_gt
K = np.float32([calib[0], 0., calib[2], 0., calib[1], calib[3], 0., 0., 1.]).reshape(3, 3)
# retval, orvec, otvec = cv2.solvePnP(obj_points, est_points, K, None, None, None, False, cv2.SOLVEPNP_ITERATIVE)
retval, orvec, otvec, inliers = cv2.solvePnPRansac(objectPoints=obj_points,
imagePoints=est_points, cameraMatrix=K,
distCoeffs=None, rvec=None, tvec=None,
useExtrinsicGuess=False, iterationsCount=100,
reprojectionError=5.0, confidence=0.99,
flags=cv2.SOLVEPNP_ITERATIVE)
R_est, _ = cv2.Rodrigues(orvec)
t_est = otvec
R_gt = pose_gt[3:]
R_gt = tf3d.euler.euler2mat(R_gt[0], R_gt[1], R_gt[2])
R_gt = np.array(R_gt, dtype=np.float32).reshape(3,3)
t_gt = pose_gt[:3]
t_gt = np.array(t_gt, dtype=np.float32) * 0.001
name_template = '00000'
len_img_id = len(str(gt['image_id']))
img_id = name_template[:-len_img_id] + str(gt['image_id'])
