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'])