def world_to_rgb(self,pt,use_distortion=True): projMatrix = np.matrix(self.rgb_camera_info.P).reshape(3,4) distCoeffs = np.matrix(self.rgb_camera_info.D) cameraMatrix, rotMatrix, tvec, _, _, _, _ = cv2.decomposeProjectionMatrix(projMatrix) rvec,_ = cv2.Rodrigues(rotMatrix) if not use_distortion: distCoeffs = np.array([]) imgpoints2, _ = cv2.projectPoints(np.array([pt]), rvec, np.zeros(3),cameraMatrix, distCoeffs) result = imgpoints2[0][0] return result
def estimatePose(self, img, corners): # Corners start from the top right corner, move downwards to the bottom # row, then starts at the top of the column to the left. # Points are in the form [column, row] # Form the world points with X pointing forward, and Y pointing left pts_3d = np.array([]).reshape(0, 3) for n in range(self.size[1]): y = n*self.square for m in range(self.size[0]): x = -m*self.square pts_3d = np.vstack([pts_3d, [x, y, 0]]) # Get checkerboard pose (unsubscribe once there is a valid estimate) valid, r_vec, t_vec = cv2.solvePnP(pts_3d, corners, self.K, self.D) if valid: self.image_sub.unregister() else: return # Form the inverse of the extrinsic matric (i.e. transform of camera wrt world) R_c = cv2.Rodrigues(r_vec)[0].T t_c = -np.matmul(R_c, t_vec) T_c = np.hstack([R_c, t_c]) alpha, beta, gamma = cv2.decomposeProjectionMatrix(T_c)[-1] # Convert euler angles to roll-pitch-yaw of a camera with X forward and Y left roll = beta pitch = -alpha - 90 yaw = gamma + 90 height = t_c[2] # Output the results print("\n--- Pose Estimation Results ---") print(" Height: %9.5f m" % height) print(" Roll: %+9.5f deg" % roll) print(" Pitch: %+9.5f deg" % pitch) print(" Yaw: %+9.5f deg\n" % yaw) # Exit when done cv2.destroyAllWindows() rospy.signal_shutdown("Done.")
def fill_polygon(blankimage, lines, indent_right, indent_left, p, factor): K, R, t_, _, _, _, _ = cv2.decomposeProjectionMatrix(p) t = np.asmatrix(-R) * np.asmatrix(t_[:3] / t_[3]) for l in lines: L = np.hstack((R, t)) * l if np.linalg.norm(L[:, 0]) < 25: if np.any(L[2, :] < 0): x0, y0 = get_intersection(L[:, 0].T, L[:, 1].T, 0.051) L[:, 0] = np.array([[x0], [y0], [0.051]]) if np.linalg.norm(L[:, 0]) > 15: W = indent(L, indent_right * factor * 0.75, indent_left * factor * 0.75, 0) else: W = indent(L, indent_right, indent_left, 0.0) x = np.dot(K, W) x = np.dot(K, L) mask = x[2, :] > 0 x = x[:2, mask] / x[2, mask] x = x.T if x.size == 8: cv2.fillPoly(blankimage, np.int32([x]), (255, 255, 255)) return blankimage
def solve(tag, imagePoints): print(tag) print("imagepoints") rprint(imagePoints) ret, rvec, tvec = cv2.solvePnP(quad_3d, imagePoints , K, dist_coef) print("R:") #print(rvec) rprint(cv2.Rodrigues(rvec)[0]) #print("tvec") print("T:") rprint(tvec) projectionMatrix = np.concatenate((cv2.Rodrigues(rvec)[0], tvec),axis=1) #print(projectionMatrix) #print("decomposed") (cam,rot,tr,rx,ry,rz,eu)=cv2.decomposeProjectionMatrix(projectionMatrix) #print('cam:') #rprint(cam) #print("T:") #rprint(tr) print("R-decomp:") rprint(rot) #print(ry) print('Euler') print('degs') rprint(eu) print('rads') rprint(eu*pi/180.0) print('heading') rprint(atan2(tvec[0],tvec[2])*180.0/pi)
def from_P(cls, P, width, height): K, R, T, Rx, Ry, Rz, angles = cv2.decomposeProjectionMatrix(P) # pylint: disable=unused-variable return cls(K=K, R=R, T=Point3D(-R @ Point3D(T)), width=width, height=height)
def getAngles(self, rvec, tvec): rmat = cv2.Rodrigues(rvec)[0] P = np.hstack((rmat, tvec)) # projection matrix [R | t] degrees = -cv2.decomposeProjectionMatrix(P)[6] print("\ndegrees:\n {0}".format(degrees)) rx, ry, rz = degrees[:, 0] return [rx, ry, rz]
def callback(msg): points = msg.data points_2d = np.array([[points[0], points[1]], [points[2], points[3]], [points[4], points[5]], [points[6], points[7]], [points[8], points[9]], [points[10], points[11]]]) #points_2d = np.array([[1060.0,340.0], [660.0,340.0], [660.0,740.0], [1060.0,740.0]]) ret, rvec, tvec = cv2.solvePnP(points_3d, points_2d, camera_mat, dist) R = cv2.Rodrigues(rvec)[0] projMat = np.array( [[R[0][0], R[0][1], R[0][2], 0.0], [R[1][0], R[1][1], R[1][2], 0.0], [R[2][0], R[2][1], R[2][2], 1.0]], dtype=np.float32) eulerAngles = cv2.decomposeProjectionMatrix(projMat)[6] print eulerAngles br.sendTransform( (-tvec[0], -tvec[1], tvec[2]), tf.transformations.quaternion_from_euler(math.radians(-eulerAngles[0]), 0, math.radians(eulerAngles[2])), rospy.Time.now(), "bebop2", "map") #print tvec #print rvec print "\n"
def readKittiCalib(path): calib = Calib() with open(path) as f: for line in f: l = Label() lines = line.split() vector = np.asarray(lines[1:], dtype=np.float32) if len(lines) == 0: continue if lines[0] == "P2:": calib.p_left = vector.reshape(3, 4) elif lines[0] == "P3:": calib.p_right = vector.reshape(3, 4) elif lines[0] == "R0_rect:": calib.r0 = vector.reshape(3, 3) calib.tx = float(calib.p_right[0, 3]) rot = np.zeros((3, 3)) trans = np.zeros((4, 1)) cam_matrix = np.zeros((3, 3), dtype=np.double) cam_matrix = cv2.decomposeProjectionMatrix(calib.p_left, cam_matrix, rot, trans) calib.k_left = cam_matrix[0] return calib
def get_head_pose(flandmarks): ''' Estimates the 3d orientation of the head. Estimates the head pose by utilizing cv2.solvePnP(). :param flandmarks: 2D facial landmarks :return: the euler angles of rotation of the persons head, a rotation matrix, translation vector, pose matrix ''' # head model from 2d facial landmarks head_model_2d = np.float32([ flandmarks[0], flandmarks[4], flandmarks[6], flandmarks[12], flandmarks[16], flandmarks[17], flandmarks[26], flandmarks[27], flandmarks[30], flandmarks[33], flandmarks[36], flandmarks[39], flandmarks[42], flandmarks[45], flandmarks[62] ]) # in camera coords _, rotation_vec, translation_vec = cv2.solvePnP(HEAD_MODEL_3D[0:15], head_model_2d, cp.cam_intrinsic_mat, cp.dist_coeffs) # transform rotation_mat, _ = cv2.Rodrigues(rotation_vec) pose_mat = cv2.hconcat((rotation_mat, translation_vec)) # transform to euler _, _, _, _, _, _, euler_angles = cv2.decomposeProjectionMatrix(pose_mat) return euler_angles, rotation_mat, translation_vec, pose_mat
def get_cam_param(filename): fs = cv2.FileStorage(filename, cv2.FILE_STORAGE_READ) if not fs.isOpened(): print("File not open") # intrinsic matrix all_cam_param = [] for i in range(num_cams): s_index = str(i).zfill(3) # projection matrix P = fs.getNode("viff" + s_index + "_matrix").mat() K, R, t = cv2.decomposeProjectionMatrix(P)[0:3] t = np.squeeze(t) np.divide(t, t[3], out=t) # print(T.shape) t = t[0:-1] cam_params = CameraParams(R, t, K, P) all_cam_param.append(cam_params) # print(array_T) fs.release() # fs = cv2.FileStorage("squirrel.yml", cv2.FILE_STORAGE_WRITE) # fs.write("Rotations", "[") # fs.write("Rotations", np.array(array_R)) # fs.write("Rotations", "]") # fs.write("Motions", np.array(array_T)) # fs.release() return all_cam_param
def aruco(self, frame): #gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) corners, ids, rejectedImgPoints = aruco.detectMarkers( frame, self.aruco_dict, parameters=self.parameters) frame_markers = aruco.drawDetectedMarkers(frame.copy(), corners, ids) rvecs, tvecs, trash = aruco.estimatePoseSingleMarkers( corners, self.size_of_marker, self.mtx, self.dist) imaxis = aruco.drawDetectedMarkers(frame.copy(), corners, ids) if tvecs is not None: for i in range(len(tvecs)): imaxis = aruco.drawAxis(imaxis, self.mtx, self.dist, rvecs[i], tvecs[i], self.length_of_axis) rvec = np.squeeze(rvecs[0], axis=None) tvec = np.squeeze(tvecs[0], axis=None) tvec = np.expand_dims(tvec, axis=1) rvec_matrix = cv2.Rodrigues(rvec)[0] proj_matrix = np.hstack((rvec_matrix, tvec)) euler_angles = cv2.decomposeProjectionMatrix(proj_matrix)[6] cv2.putText(imaxis, 'X: ' + str(int(euler_angles[0])), (10, 30), cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (0, 0, 255)) cv2.putText(imaxis, 'Y: ' + str(int(euler_angles[1])), (115, 30), cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (0, 255, 0)) cv2.putText(imaxis, 'Z: ' + str(int(euler_angles[2])), (200, 30), cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (255, 0, 0)) cv2.imshow('Aruco', imaxis)
def head_pose(model_points, sorted_image_points, camera_matrix): dist_coeffs = np.zeros((4,1)) # Assuming no lens distortion (success, rotation_vector, translation_vector) = cv2.solvePnP(model_points, sorted_image_points, camera_matrix, dist_coeffs, flags=cv2.SOLVEPNP_ITERATIVE) rotation_mat, _ = cv2.Rodrigues(rotation_vector) pose_mat = cv2.hconcat((rotation_mat, translation_vector)) _, _, _, _, _, _, euler_angle = cv2.decomposeProjectionMatrix(pose_mat) return euler_angle
def cvtToBoxPnp(self, box_info): """数据格式转换,转换至目标位姿信息""" w, h = self.armour_size w, h = w / 2, h / 2 world_coordinate = np.array([ [w, h, 0], [-w, h, 0], [-w, -h, 0], [w, -h, 0], ], dtype=np.float64) # 像素坐标 pnts = np.array(box_info, dtype=np.float64) # rotation_vector 旋转向量 translation_vector 平移向量 success, rvec, tvec = cv2.solvePnP(world_coordinate, pnts, Camera_intrinsic["mtx"], Camera_intrinsic["dist"]) distance = np.linalg.norm(tvec) yaw_angle = np.arctan(tvec[0] / tvec[2]) pitch_angle = np.arctan(tvec[1] / tvec[2]) # 默认为弧度制,转换为角度制改下面 # 这里角度为像素坐标系值,图像中右侧为x正方向,下侧为y轴正方向 yaw_angle = float(np.rad2deg(yaw_angle)) pitch_angle = -float(np.rad2deg(pitch_angle)) rvec_matrix = cv2.Rodrigues(rvec)[0] proj_matrix = np.hstack((rvec_matrix, rvec)) eulerAngles = -cv2.decomposeProjectionMatrix(proj_matrix)[6] # 欧拉角 pitch, roll, yaw = eulerAngles[0], eulerAngles[1], eulerAngles[2] rect_pnp_info = [distance, int(yaw_angle), int(pitch_angle)] return rect_pnp_info
def get_head_pose(shape, im_shape): K = [ im_shape[1], 0.0, im_shape[1] / 2, 0.0, im_shape[1], im_shape[0] / 2, 0.0, 0.0, 1.0 ] D = [0.0, 0.0, 0.0, 0.0] cam_matrix = np.array(K).reshape(3, 3).astype(np.float32) dist_coeffs = np.array(D).reshape(4, 1).astype(np.float32) # 选取68个关键点中的14个点 image_pts = np.float32([ shape[17], shape[21], shape[22], shape[26], shape[36], shape[39], shape[42], shape[45], shape[31], shape[35], shape[48], shape[54], shape[57], shape[8] ]) _, rotation_vec, translation_vec = cv2.solvePnP(object_pts, image_pts, cam_matrix, dist_coeffs) reprojectdst, _ = cv2.projectPoints(reprojectsrc, rotation_vec, translation_vec, cam_matrix, dist_coeffs) reprojectdst = tuple(map(tuple, reprojectdst.reshape(8, 2))) # calc euler angle rotation_mat, _ = cv2.Rodrigues(rotation_vec) pose_mat = cv2.hconcat((rotation_mat, translation_vec)) _, _, _, _, _, _, euler_angle = cv2.decomposeProjectionMatrix(pose_mat) return reprojectdst, euler_angle
def face_orientation(self, landmarks): v_cx, v_cy = landmarks[ 2, 0] - (landmarks[0, 0] + landmarks[1, 0]) / 2, landmarks[ 2, 1] - (landmarks[0, 1] + landmarks[1, 1]) / 2 image_points = np.array( [ (landmarks[2, 0], landmarks[2, 1]), # Nose tip (landmarks[2, 0] + 1.2 * v_cx, landmarks[2, 1] + 1.2 * v_cy), # Chin (landmarks[0, 0], landmarks[0, 1]), # Left eye left corner (landmarks[1, 0], landmarks[1, 1]), # Right eye right corne (landmarks[3, 0], landmarks[3, 1]), # Left Mouth corner (landmarks[4, 0], landmarks[4, 1]) # Right mouth corner ], dtype="double") (success, rotation_vector, translation_vector) = cv2.solvePnP(self.model_points, image_points, self.camera_matrix, self.dist_coeffs) rvec_matrix = cv2.Rodrigues(rotation_vector)[0] proj_matrix = np.hstack((rvec_matrix, translation_vector)) eulerAngles = cv2.decomposeProjectionMatrix(proj_matrix)[6] pitch, yaw, roll = [math.radians(_) for _ in eulerAngles] pitch = math.degrees(math.asin(math.sin(pitch))) roll = -math.degrees(math.asin(math.sin(roll))) yaw = math.degrees(math.asin(math.sin(yaw))) return int(roll), int(pitch), int(yaw)
def get_euler_angle(self, shape, frame): gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # get the shape of the image size = gray.shape # find the landmark image points image_points = np.float32([shape[17], shape[21], shape[22], shape[26], shape[36], shape[39], shape[42], shape[45], shape[31], shape[33], shape[35], shape[48], shape[54], shape[57], shape[8]]) # calculate the camera matrix and declare coefficients (we assume no lens distortion) 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)) # calculate the Rigid Body Transform vectors _, rotation_vec, translation_vec = cv2.solvePnP(self.model_points, image_points, camera_matrix, dist_coeffs) # calc euler angle rotation_mat, _ = cv2.Rodrigues(rotation_vec) pose_mat = cv2.hconcat((rotation_mat, translation_vec)) _, _, _, _, _, _, euler_angle = cv2.decomposeProjectionMatrix(pose_mat) return euler_angle
def calculate_pitch_yaw_roll(landmarks_2D, cam_w=256, cam_h=256, radians=False): """ Return the the pitch yaw and roll angles associated with the input image. @param radians When True it returns the angle in radians, otherwise in degrees. """ assert landmarks_2D is not None, 'landmarks_2D is None' # Estimated camera matrix values. c_x = cam_w / 2 c_y = cam_h / 2 f_x = c_x / np.tan(60 / 2 * np.pi / 180) f_y = f_x camera_matrix = np.float32([[f_x, 0.0, c_x], [0.0, f_y, c_y], [0.0, 0.0, 1.0]]) camera_distortion = np.float32([0.0, 0.0, 0.0, 0.0, 0.0]) # dlib (68 landmark) trached points # TRACKED_POINTS = [17, 21, 22, 26, 36, 39, 42, 45, 31, 35, 48, 54, 57, 8] # wflw(98 landmark) trached points # TRACKED_POINTS = [33, 38, 50, 46, 60, 64, 68, 72, 55, 59, 76, 82, 85, 16] # X-Y-Z with X pointing forward and Y on the left and Z up. # The X-Y-Z coordinates used are like the standard coordinates of ROS (robotic operative system) # OpenCV uses the reference usually used in computer vision: # X points to the right, Y down, Z to the front landmarks_3D = np.float32([ [6.825897, 6.760612, 4.402142], # LEFT_EYEBROW_LEFT, [1.330353, 7.122144, 6.903745], # LEFT_EYEBROW_RIGHT, [-1.330353, 7.122144, 6.903745], # RIGHT_EYEBROW_LEFT, [-6.825897, 6.760612, 4.402142], # RIGHT_EYEBROW_RIGHT, [5.311432, 5.485328, 3.987654], # LEFT_EYE_LEFT, [1.789930, 5.393625, 4.413414], # LEFT_EYE_RIGHT, [-1.789930, 5.393625, 4.413414], # RIGHT_EYE_LEFT, [-5.311432, 5.485328, 3.987654], # RIGHT_EYE_RIGHT, [-2.005628, 1.409845, 6.165652], # NOSE_LEFT, [-2.005628, 1.409845, 6.165652], # NOSE_RIGHT, [2.774015, -2.080775, 5.048531], # MOUTH_LEFT, [-2.774015, -2.080775, 5.048531], # MOUTH_RIGHT, [0.000000, -3.116408, 6.097667], # LOWER_LIP, [0.000000, -7.415691, 4.070434], # CHIN ]) landmarks_2D = np.asarray(landmarks_2D, dtype=np.float32).reshape(-1, 2) # Applying the PnP solver to find the 3D pose of the head from the 2D position of the landmarks. # retval - bool # rvec - Output rotation vector that, together with tvec, brings points from the world coordinate system to the camera coordinate system. # tvec - Output translation vector. It is the position of the world origin (SELLION) in camera co-ords _, rvec, tvec = cv2.solvePnP(landmarks_3D, landmarks_2D, camera_matrix, camera_distortion) #Get as input the rotational vector, Return a rotational matrix # const double PI = 3.141592653; # double thetaz = atan2(r21, r11) / PI * 180; # double thetay = atan2(-1 * r31, sqrt(r32*r32 + r33*r33)) / PI * 180; # double thetax = atan2(r32, r33) / PI * 180; rmat, _ = cv2.Rodrigues(rvec) pose_mat = cv2.hconcat((rmat, tvec)) _, _, _, _, _, _, euler_angles = cv2.decomposeProjectionMatrix(pose_mat) return map(lambda k: k[0], euler_angles) # euler_angles contain (pitch, yaw, roll)
def solve_pose_by_68_points(self, image_points): """ Solve pose from all the 68 image points Return (rotation_vector, translation_vector) as pose. """ if self.r_vec is None: (_, rotation_vector, translation_vector) = cv2.solvePnP( self.model_points_68, image_points, self.camera_matrix, self.dist_coeefs) self.r_vec = rotation_vector self.t_vec = translation_vector (_, rotation_vector, translation_vector) = cv2.solvePnP( self.model_points_68, image_points, self.camera_matrix, self.dist_coeefs, rvec=self.r_vec, tvec=self.t_vec, useExtrinsicGuess=True) axis = np.float32([[500,0,0], [0,500,0], [0,0,500]]) imgpts, jac = cv2.projectPoints(axis, rotation_vector, translation_vector, self.camera_matrix, self.dist_coeefs) modelpts, jac2 = cv2.projectPoints(self.model_points, rotation_vector, translation_vector, self.camera_matrix, self.dist_coeefs) rvec_matrix = cv2.Rodrigues(rotation_vector)[0] proj_matrix = np.hstack((rvec_matrix, translation_vector)) eulerAngles = cv2.decomposeProjectionMatrix(proj_matrix)[6] pitch, yaw, roll = [math.radians(_) for _ in eulerAngles] pitch = math.degrees(math.asin(math.sin(pitch))) roll = -math.degrees(math.asin(math.sin(roll))) yaw = math.degrees(math.asin(math.sin(yaw))) return (rotation_vector, translation_vector),(round(yaw,2), round(pitch,2), round(roll,2))
def face_orientation(frame, landmarks): size = frame.shape image_points = landmarks.astype('double') model_points = np.array([ (0.0, 0.0, 0.0), # Nose tip (0.0, -330.0, -65.0), # Chin (-165.0, 170.0, -135.0), # Left eye left corner (165.0, 170.0, -135.0), # Right eye right corne (-150.0, -150.0, -125.0), # Left Mouth corner (150.0, -150.0, -125.0) # Right mouth corner ]) center = (size[1]/2, size[0]/2) focal_length = center[0] / np.tan(60/2 * np.pi / 180) 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)) (success, rotation_vector, translation_vector) = cv2.solvePnP(model_points, image_points, camera_matrix, dist_coeffs, flags=cv2.SOLVEPNP_ITERATIVE) axis = np.float32([[500,0,0], [0,500,0], [0,0,500]]) imgpts, jac = cv2.projectPoints(axis, rotation_vector, translation_vector, camera_matrix, dist_coeffs) modelpts, jac2 = cv2.projectPoints(model_points, rotation_vector, translation_vector, camera_matrix, dist_coeffs) rvec_matrix = cv2.Rodrigues(rotation_vector)[0] proj_matrix = np.hstack((rvec_matrix, translation_vector)) eulerAngles = cv2.decomposeProjectionMatrix(proj_matrix)[6] pitch, yaw, roll = [np.radians(_) for _ in eulerAngles] pitch = np.degrees(np.arcsin(np.sin(pitch))) roll = -np.degrees(np.arcsin(np.sin(roll))) yaw = np.degrees(np.arcsin(np.sin(yaw))) return (str(float("{0:.2f}".format(roll[0]))), str(float("{0:.2f}".format(pitch[0]))), str(float("{0:.2f}".format(yaw[0]))))
def get_head_pose(shape): image_pts = np.float32([ shape[17], shape[21], shape[22], shape[26], shape[36], shape[39], shape[42], shape[45], shape[31], shape[35], shape[48], shape[54], shape[57], shape[8] ]) _, rotation_vec, translation_vec = cv2.solvePnP(object_pts, image_pts, cam_matrix, dist_coeffs) print(rotation_vec.shape, translation_vec.shape) reprojectdst, _ = cv2.projectPoints(reprojectsrc, rotation_vec, translation_vec, cam_matrix, dist_coeffs) reprojectdst = tuple(map(tuple, reprojectdst.reshape(8, 2))) # calc euler angle rotation_mat, _ = cv2.Rodrigues(rotation_vec) print(rotation_mat.shape) pose_mat = cv2.hconcat((rotation_mat, translation_vec)) print(pose_mat.shape) _, _, _, _, _, _, euler_angle = cv2.decomposeProjectionMatrix(pose_mat) return reprojectdst, euler_angle
def get_head_pose(shape):# 头部姿态估计 # (像素坐标集合)填写2D参考点,注释遵循https://ibug.doc.ic.ac.uk/resources/300-W/ # 17左眉左上角/21左眉右角/22右眉左上角/26右眉右上角/36左眼左上角/39左眼右上角/42右眼左上角/ # 45右眼右上角/31鼻子左上角/35鼻子右上角/48左上角/54嘴右上角/57嘴中央下角/8下巴角 image_pts = np.float32([shape[17], shape[21], shape[22], shape[26], shape[36], shape[39], shape[42], shape[45], shape[31], shape[35], shape[48], shape[54], shape[57], shape[8]]) # solvePnP计算姿势——求解旋转和平移矩阵: # rotation_vec表示旋转矩阵,translation_vec表示平移矩阵,cam_matrix与K矩阵对应,dist_coeffs与D矩阵对应。 _, rotation_vec, translation_vec = cv2.solvePnP(object_pts, image_pts, cam_matrix, dist_coeffs) # projectPoints重新投影误差:原2d点和重投影2d点的距离(输入3d点、相机内参、相机畸变、r、t,输出重投影2d点) reprojectdst, _ = cv2.projectPoints(reprojectsrc, rotation_vec, translation_vec, cam_matrix,dist_coeffs) reprojectdst = tuple(map(tuple, reprojectdst.reshape(8, 2)))# 以8行2列显示 # 计算欧拉角calc euler angle # 参考https://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html#decomposeprojectionmatrix rotation_mat, _ = cv2.Rodrigues(rotation_vec)#罗德里格斯公式(将旋转矩阵转换为旋转向量) pose_mat = cv2.hconcat((rotation_mat, translation_vec))# 水平拼接,vconcat垂直拼接 # decomposeProjectionMatrix将投影矩阵分解为旋转矩阵和相机矩阵 _, _, _, _, _, _, euler_angle = cv2.decomposeProjectionMatrix(pose_mat) pitch, yaw, roll = [math.radians(_) for _ in euler_angle] pitch = math.degrees(math.asin(math.sin(pitch))) roll = -math.degrees(math.asin(math.sin(roll))) yaw = math.degrees(math.asin(math.sin(yaw))) print('pitch:{}, yaw:{}, roll:{}'.format(pitch, yaw, roll)) return reprojectdst, euler_angle# 投影误差,欧拉角
def face_orientation(frame, landmarks): size = frame.shape # (height, width, color_channel) image_points = np.array( [ (landmarks[4], landmarks[5]), # Nose tip # (landmarks[10], landmarks[11]), # Chin (landmarks[0], landmarks[1]), # Left eye left corner (landmarks[2], landmarks[3]), # Right eye right corne (landmarks[6], landmarks[7]), # Left Mouth corner (landmarks[8], landmarks[9]) # Right mouth corner ], dtype="double") model_points = np.array([ (0.0, 0.0, 0.0), # Nose tip # (0.0, -330.0, -65.0), # Chin (-165.0, 170.0, -135.0), # Left eye left corner (165.0, 170.0, -135.0), # Right eye right corne (-150.0, -150.0, -125.0), # Left Mouth corner (150.0, -150.0, -125.0) # Right mouth corner ]) # Camera internals center = (size[1] / 2, size[0] / 2) focal_length = center[0] / np.tan(60 / 2 * np.pi / 180) camera_matrix = np.array([[focal_length, 0, center[0]], [0, focal_length, center[1]], [0, 0, 1]], dtype="double") dist_coeffs = np.zeros((4, 1)) # Assuming no lens distortion (success, rotation_vector, translation_vector) = cv2.solvePnP( model_points, image_points, camera_matrix, dist_coeffs, # flags=cv2.SOLVEPNP_ITERATIVE ) axis = np.float32([[500, 0, 0], [0, 500, 0], [0, 0, 500]]) imgpts, jac = cv2.projectPoints(axis, rotation_vector, translation_vector, camera_matrix, dist_coeffs) modelpts, jac2 = cv2.projectPoints(model_points, rotation_vector, translation_vector, camera_matrix, dist_coeffs) rvec_matrix = cv2.Rodrigues(rotation_vector)[0] proj_matrix = np.hstack((rvec_matrix, translation_vector)) eulerAngles = cv2.decomposeProjectionMatrix(proj_matrix)[6] pitch, yaw, roll = [math.radians(_) for _ in eulerAngles] pitch = math.degrees(math.asin(math.sin(pitch))) roll = -math.degrees(math.asin(math.sin(roll))) yaw = math.degrees(math.asin(math.sin(yaw))) return imgpts, modelpts, (str(int(roll)), str(int(pitch)), str(int(yaw))), (landmarks[4], landmarks[5])
def _load_rt(self, path): p = self._load_projection_matrix(path) res = cv2.decomposeProjectionMatrix(p) cameraMatrix, rotMatrix, transVect = res[:3] R = rotMatrix.astype('float64') T = (transVect.astype('float64') / transVect.astype('float64')[3])[:3] return R, T
def GetDistanceAndAngle(self, im, rvecs, tvecs, QRsize=13): """Use rotation and translation vectors and size of a QR code to determan euler angles and distance""" #get distance from matrix dis = (tvecs.ravel()[2] * QRsize).item() # calc euler angle rotation_mat, _ = cv2.Rodrigues(rvecs) pose_mat = cv2.hconcat((rotation_mat, tvecs)) _, _, _, _, _, _, euler_angle = cv2.decomposeProjectionMatrix(pose_mat) if (self.visualizeResult): cv2.putText(im, ("Distance: " + str(round(dis, 2)) + "cm"), (5, 25), font, 1, (255, 50, 50), 2) cv2.putText( im, ("angles X: " + str(round(euler_angle[0][0], 2)) + "degree"), (5, 65), font, 1, (255, 50, 50), 2) cv2.putText( im, ("angles Y: " + str(round(euler_angle[1][0], 2)) + "degree"), (5, 105), font, 1, (255, 50, 50), 2) cv2.putText( im, ("angles Z: " + str(round(euler_angle[2][0], 2)) + "degree"), (5, 145), font, 1, (255, 50, 50), 2) if (self.debug): print("Distance: " + str(dis) + "|AngX: " + str(euler_angle[0][0]) + "|AngY: " + str(euler_angle[1][0]) + "|AngZ: " + str(euler_angle[2][0])) return dis, euler_angle
def face_orientation(frame, landmarks): size = frame.shape #(height, width, color_channel) image_points = np.array( [ landmarks["30"], # Nose landmarks["8"], # Chin landmarks["45"], # Left eye left corner landmarks["36"], # Right eye right cornee landmarks["54"], # Left Mouth corner landmarks["48"] # Right mouth corner ], dtype="double") # --------------------------------- # (441,649), # Nose tip # (415,924), # Chin # (574,534), # Left eye left corner # (278,513), # Right eye right corne # (528,729), # Left Mouth corner # (305,714) # Right mouth corner model_points = np.array([ (0.0, 0.0, 0.0), # Nose tip (0.0, -330.0, -65.0), # Chin (-165.0, 170.0, -135.0), # Left eye left corner (165.0, 170.0, -135.0), # Right eye right corne (-150.0, -150.0, -125.0), # Left Mouth corner (150.0, -150.0, -125.0) # Right mouth corner ]) center = (size[1] / 2, size[0] / 2) focal_length = center[0] / np.tan(60 / 2 * np.pi / 180) camera_matrix = np.array([[focal_length, 0, center[0]], [0, focal_length, center[1]], [0, 0, 1]], dtype="double") dist_coeffs = np.zeros((4, 1)) # Assuming no lens distortion (success, rotation_vector, translation_vector) = cv2.solvePnP(model_points, image_points, camera_matrix, dist_coeffs) axis = np.float32([[500, 0, 0], [0, 500, 0], [0, 0, 500]]) imgpts, jac = cv2.projectPoints(axis, rotation_vector, translation_vector, camera_matrix, dist_coeffs) modelpts, jac2 = cv2.projectPoints(model_points, rotation_vector, translation_vector, camera_matrix, dist_coeffs) rvec_matrix = cv2.Rodrigues(rotation_vector)[0] proj_matrix = np.hstack((rvec_matrix, translation_vector)) eulerAngles = cv2.decomposeProjectionMatrix(proj_matrix)[6] pitch, yaw, roll = [math.radians(_) for _ in eulerAngles] pitch = math.degrees(math.asin(math.sin(pitch))) roll = -math.degrees(math.asin(math.sin(roll))) yaw = math.degrees(math.asin(math.sin(yaw))) return imgpts, modelpts, ((int(roll)), (int(pitch)), (int(yaw))), (landmarks["4"], landmarks["5"])
def get_head_pose(shape, frame_counter, frames, pitch_angles, roll_angles, yaw_angles): image_pts = np.float32([ shape[17], shape[21], shape[22], shape[26], shape[36], shape[39], shape[42], shape[45], shape[31], shape[35], shape[48], shape[54], shape[57], shape[8] ]) _, rotation_vec, translation_vec = cv2.solvePnP(object_pts, image_pts, cam_matrix, dist_coeffs) reprojectdst, _ = cv2.projectPoints(reprojectsrc, rotation_vec, translation_vec, cam_matrix, dist_coeffs) reprojectdst = tuple(map(tuple, reprojectdst.reshape(8, 2))) # calc euler angle rotation_mat, _ = cv2.Rodrigues(rotation_vec) pose_mat = cv2.hconcat((rotation_mat, translation_vec)) _, _, _, _, _, _, euler_angle = cv2.decomposeProjectionMatrix(pose_mat) pitch = euler_angle[0] roll = euler_angle[1] yaw = euler_angle[2] frames.append(frame_counter) pitch_angles.append(pitch) roll_angles.append(roll) yaw_angles.append(yaw) frame_counter += 1 # print(pitch) return reprojectdst, euler_angle, frame_counter, frames, pitch_angles, roll_angles, yaw_angles
def load_K_Rt_from_P(filename, P=None): if P is None: lines = open(filename).read().splitlines() if len(lines) == 4: lines = lines[1:] lines = [[x[0], x[1], x[2], x[3]] for x in (x.split(" ") for x in lines)] P = np.asarray(lines).astype(np.float32).squeeze() out = cv2.decomposeProjectionMatrix(P) K = out[0] R = out[1] t = out[2] K = K / K[2, 2] intrinsics = np.eye(4) intrinsics[:3, :3] = K pose = np.eye(4, dtype=np.float32) to_gl = np.eye(3, dtype=np.float32) to_gl[0, 0] = -1. to_gl[1, 1] = -1. pose[:3, :3] = np.dot(R.transpose(), to_gl) pose[:3, 3] = (t[:3] / t[3])[:, 0] return intrinsics, pose
def get_head_pose(shape, img): h, w, _ = img.shape K = [w, 0.0, w // 2, 0.0, w, h // 2, 0.0, 0.0, 1.0] # Assuming no lens distortion D = [0, 0, 0.0, 0.0, 0] cam_matrix = np.array(K).reshape(3, 3).astype(np.float32) dist_coeffs = np.array(D).reshape(5, 1).astype(np.float32) # image_pts = np.float32([shape[17], shape[21], shape[22], shape[26], shape[36], # shape[39], shape[42], shape[45], shape[31], shape[35], # shape[48], shape[54], shape[57], shape[8]]) image_pts = np.float32([ shape[17], shape[21], shape[22], shape[26], shape[36], shape[39], shape[42], shape[45], shape[31], shape[35] ]) _, rotation_vec, translation_vec = cv2.solvePnP(object_pts, image_pts, cam_matrix, dist_coeffs) reprojectdst, _ = cv2.projectPoints(reprojectsrc, rotation_vec, translation_vec, cam_matrix, dist_coeffs) reprojectdst = tuple(map(tuple, reprojectdst.reshape(8, 2))) # calc euler angle rotation_mat, _ = cv2.Rodrigues(rotation_vec) pose_mat = cv2.hconcat((rotation_mat, translation_vec)) _, _, _, _, _, _, euler_angle = cv2.decomposeProjectionMatrix(pose_mat) return reprojectdst, euler_angle
def _get_pitch_yaw(self): """ Obtain the yaw and pitch from the :attr:`_rotation` in eular angles. """ proj_matrix = np.zeros((3, 4), dtype="float32") proj_matrix[:3, :3] = cv2.Rodrigues(self._rotation)[0] euler = cv2.decomposeProjectionMatrix(proj_matrix)[-1] self._pitch_yaw = (euler[0][0], euler[1][0]) logger.trace("yaw_pitch: %s", self._pitch_yaw)
def getFaceDirection(self, rotateMatrix, tVec): rotateMatrix = np.array(rotateMatrix) tVec = np.array(tVec).T projectionMatrix = np.concatenate((rotateMatrix, tVec), axis=1) _, _, _, _, _, _, eulerAngles = cv2.decomposeProjectionMatrix( projectionMatrix) print(eulerAngles)
def get_head_pose(shape): # Dlib shape_predictor can get 68 points of face image_pts = np.float32([ shape[17], shape[21], shape[22], shape[26], shape[36], shape[39], shape[42], shape[45], shape[31], shape[35], shape[48], shape[54], shape[57], shape[8] ]) _, rotation_vec, translation_vec = cv2.solvePnP(pose.object_pts, image_pts, pose.cam_matrix, pose.dist_coeffs) # Project 3D points to image plane , output image points reprojectdst, _ = cv2.projectPoints(pose.reprojectsrc, rotation_vec, translation_vec, pose.cam_matrix, pose.dist_coeffs) # Reprojected 2D points reprojectdst = tuple(map(tuple, reprojectdst.reshape(8, 2))) # Calc euler angle rotation_mat, _ = cv2.Rodrigues(rotation_vec) pose_mat = cv2.hconcat((rotation_mat, translation_vec)) _, _, _, _, _, _, euler_angle = cv2.decomposeProjectionMatrix(pose_mat) return reprojectdst, euler_angle
def get_head_pose(shape, cam_matrix, dist_coeffs): #image points are the landmark fiducial points on the image plane corresponding to the 3D model points (model_pts) image_pts = np.float32( [shape[30], shape[8], shape[36], shape[45], shape[48], shape[54]]) #OpenCV solvepnp function to compute head pose (success, rotation_vec, translation_vec) = cv2.solvePnP( model_pts, image_pts, cam_matrix, dist_coeffs, flags=cv2.cv2.SOLVEPNP_ITERATIVE) #, flags=cv2.CV_ITERATIVE) reprojectdst, _ = cv2.projectPoints(reprojectsrc, rotation_vec, translation_vec, cam_matrix, dist_coeffs) reprojectdst = tuple(map(tuple, reprojectdst.reshape(8, 2))) # calc euler angle rotation_mat, _ = cv2.Rodrigues(rotation_vec) R_rvec = R.from_rotvec(rotation_vec.transpose()) R_rotmat = R_rvec.as_matrix() print(rotation_mat, R_rotmat) pose_mat = cv2.hconcat((rotation_mat, translation_vec)) _, _, _, _, _, _, euler_angle = cv2.decomposeProjectionMatrix(pose_mat) return reprojectdst, rotation_vec, rotation_mat, translation_vec, euler_angle, image_pts
def get_head_pose(shape): image_pts = np.float32([shape[17], shape[21], shape[22], shape[26], shape[36], shape[39], shape[42], shape[45], shape[31], shape[35], shape[48], shape[54], shape[57], shape[8]]) _, rotation_vec, translation_vec = cv2.solvePnP(object_pts, image_pts, cam_matrix, dist_coeffs) reprojectdst, _ = cv2.projectPoints(reprojectsrc, rotation_vec, translation_vec, cam_matrix, dist_coeffs) reprojectdst = tuple(map(tuple, reprojectdst.reshape(8, 2))) # calc euler angle rotation_mat, _ = cv2.Rodrigues(rotation_vec) pose_mat = cv2.hconcat((rotation_mat, translation_vec)) _, _, _, _, _, _, euler_angle = cv2.decomposeProjectionMatrix(pose_mat) return reprojectdst, euler_angle
""" Decomposition of the projection matrix """ import cv2 import numpy as np cameraMatrix1 = np.identity(3,np.float) rotMatrix1 = np.identity(3,np.float) transVect1 = np.array([0, 0, 0, 0], np.float) projMatrix1 = np.array([[1.008625769,-.1231000000,-2.726496908,100.7731396], [.8361681672,4.182806666,.09730552466,157.7821517], [.001940031162,-.0002500000000,.0004294973553,.5091827970]], np.float) cv2.decomposeProjectionMatrix(projMatrix1, cameraMatrix1, rotMatrix1, transVect1) print "cameraMatrix: ", cameraMatrix1 print "rotMatrix", rotMatrix1 print "transVect", transVect1 cameraMatrix2 = np.identity(3,np.float) rotMatrix2 = np.identity(3,np.float) transVect2 = np.array([0, 0, 0, 0], np.float) projMatrix2 = np.array([[1.426624453,-.1471000000,-2.539954861,78.3047630], [.7976452886,4.150586666,.1303686090,143.3641217], [.001903601260,-.0001466666667,.0006532670189,.5012476834]], np.float) cv2.decomposeProjectionMatrix(projMatrix2, cameraMatrix2, rotMatrix2, transVect2) print "cameraMatrix: ", cameraMatrix2 print "rotMatrix", rotMatrix2
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
def encode6b(Tr): r,t = cv2.decomposeProjectionMatrix(Tr[:3])[1:3] return r_[cv2.Rodrigues(r)[0].flatten(), t.flat[:3]/t[3]]
def get_theta(self): __,__,__,x,y,z,euler =cv2.decomposeProjectionMatrix(self.Proj) #self.yaw = np.arccos(x[1,1])*180/np.pi #yaw #self.pitch = np.arccos(y[0,0])*180/np.pi #pitch #self.roll = np.arccos(z[0,0])*180/np.pi #roll self.yaw=euler[0] self.pitch=euler[1] self.roll=euler[2]
def calibrateImages(obj_pts, ptsL, ptsR, imsize): '''Calibrate the Stereo camera rig, given: @obj_pts: points that specify the calibration checkerboard pattern @ptsL: detected image points in the left calibration images @ptsR: detected image points in the right calibration images @imsize: the stipulated size of all calibration images return: updated StereoCam object ''' # Perform Stereo Calibration retval, cam1, dist1, cam2, dist2, R, T, E, F = \ cv2.stereoCalibrate( obj_pts, ptsL, ptsR, imsize) dist1 = dist1.ravel() dist2 = dist2.ravel() cameraObj = StereoCam() cameraObj.cam1, cameraObj.dist1 = cam1, dist1 cameraObj.cam2, cameraObj.dist2 = cam2, dist2 cameraObj.R, cameraObj.T, cameraObj.E, cameraObj.F = R, T, E, F # Perform Stereo Rectification (R1, R2, P1, P2, Q, roi1, roi2) = \ cv2.stereoRectify(cam1, dist1, cam2, dist2, imsize, R, T) # update the left and right rotation and projection matrices cameraObj.R1, cameraObj.R2 = R1, R2 cameraObj.P1, cameraObj.P2 = P1, P2 # update the image projection (aka disparity-to-depth mapping) matrix cameraObj.Q = Q # Get optimal new camera matrices from the rectified projection matrices K_L = cv2.decomposeProjectionMatrix(P1) K1 = K_L[0] K_R = cv2.decomposeProjectionMatrix(P2) K2 = K_R[0] cameraObj.K1, cameraObj.K2 = K1, K2 return cameraObj