################################################################################################## ## License: Apache 2.0. See LICENSE file in root directory. #### ################################################################################################## ## Box Dimensioner with multiple cameras: Helper files #### ################################################################################################## import pyrealsense2 as rs import calculate_rmsd_kabsch as rmsd import numpy as np from helper_functions import cv_find_chessboard, get_chessboard_points_3D, get_depth_at_pixel, convert_depth_pixel_to_metric_coordinate from realsense_device_manager import post_process_depth_frame """ _ _ _ _____ _ _ | | | | ___ | | _ __ ___ _ __ | ___|_ _ _ __ ___ | |_ (_) ___ _ __ ___ | |_| | / _ \| || '_ \ / _ \| '__| | |_ | | | || '_ \ / __|| __|| | / _ \ | '_ \ / __| | _ || __/| || |_) || __/| | | _| | |_| || | | || (__ | |_ | || (_) || | | |\__ \ |_| |_| \___||_|| .__/ \___||_| |_| \__,_||_| |_| \___| \__||_| \___/ |_| |_||___/ _| """ def calculate_transformation_kabsch(src_points, dst_points): """ Calculates the optimal rigid transformation from src_points to dst_points (regarding the least squares error) Parameters: ----------- src_points: array (3,N) matrix dst_points: array (3,N) matrix Returns: ----------- rotation_matrix: array (3,3) matrix translation_vector: array (3,1) matrix rmsd_value: float """ assert src_points.shape == dst_points.shape if src_points.shape[0] != 3: raise Exception("The input data matrix had to be transposed in order to compute transformation.") src_points = src_points.transpose() dst_points = dst_points.transpose() src_points_centered = src_points - rmsd.centroid(src_points) dst_points_centered = dst_points - rmsd.centroid(dst_points) rotation_matrix = rmsd.kabsch(src_points_centered, dst_points_centered) rmsd_value = rmsd.kabsch_rmsd(src_points_centered, dst_points_centered) translation_vector = rmsd.centroid(dst_points) - np.matmul(rmsd.centroid(src_points), rotation_matrix) return rotation_matrix.transpose(), translation_vector.transpose(), rmsd_value """ __ __ _ ____ _ _ | \/ | __ _ (_) _ __ / ___| ___ _ __ | |_ ___ _ __ | |_ | |\/| | / _` || || '_ \ | | / _ \ | '_ \ | __|/ _ \| '_ \ | __| | | | || (_| || || | | | | |___| (_) || | | || |_| __/| | | || |_ |_| |_| \__,_||_||_| |_| \____|\___/ |_| |_| \__|\___||_| |_| \__| """ class Transformation: def __init__(self, rotation_matrix, translation_vector): self.pose_mat = np.zeros((4,4)) self.pose_mat[:3,:3] = rotation_matrix self.pose_mat[:3,3] = translation_vector.flatten() self.pose_mat[3,3] = 1 def apply_transformation(self, points): """ Applies the transformation to the pointcloud Parameters: ----------- points : array (3, N) matrix where N is the number of points Returns: ---------- points_transformed : array (3, N) transformed matrix """ assert(points.shape[0] == 3) n = points.shape[1] points_ = np.vstack((points, np.ones((1,n)))) points_trans_ = np.matmul(self.pose_mat, points_) points_transformed = np.true_divide(points_trans_[:3,:], points_trans_[[-1], :]) return points_transformed def inverse(self): """ Computes the inverse transformation and returns a new Transformation object Returns: ----------- inverse: Transformation """ rotation_matrix = self.pose_mat[:3,:3] translation_vector = self.pose_mat[:3,3] rot = np.transpose(rotation_matrix) trans = - np.matmul(np.transpose(rotation_matrix), translation_vector) return Transformation(rot, trans) class PoseEstimation: def __init__(self, frames, intrinsic, chessboard_params): assert(len(chessboard_params) == 3) self.frames = frames self.intrinsic = intrinsic self.chessboard_params = chessboard_params def get_chessboard_corners_in3d(self): """ Searches the chessboard corners in the infrared images of every connected device and uses the information in the corresponding depth image to calculate the 3d coordinates of the chessboard corners in the coordinate system of the camera Returns: ----------- corners3D : dict keys: str Serial number of the device values: [success, points3D, validDepths] success: bool Indicates wether the operation was successfull points3d: array (3,N) matrix with the coordinates of the chessboard corners in the coordinate system of the camera. N is the number of corners in the chessboard. May contain points with invalid depth values validDephts: [bool]* Sequence with length N indicating which point in points3D has a valid depth value """ corners3D = {} for (info, frameset) in self.frames.items(): serial = info[0] product_line = info[1] depth_frame = post_process_depth_frame(frameset[rs.stream.depth]) infrared_frame = frameset[(rs.stream.infrared, 1)] depth_intrinsics = self.intrinsic[serial][rs.stream.depth] found_corners, points2D = cv_find_chessboard(depth_frame, infrared_frame, self.chessboard_params) corners3D[serial] = [found_corners, None, None, None] if found_corners: points3D = np.zeros((3, len(points2D[0]))) validPoints = [False] * len(points2D[0]) for index in range(len(points2D[0])): corner = points2D[:,index].flatten() depth = get_depth_at_pixel(depth_frame, corner[0], corner[1]) if depth != 0 and depth is not None: validPoints[index] = True [X,Y,Z] = convert_depth_pixel_to_metric_coordinate(depth, corner[0], corner[1], depth_intrinsics) points3D[0, index] = X points3D[1, index] = Y points3D[2, index] = Z corners3D[serial] = found_corners, points2D, points3D, validPoints return corners3D def perform_pose_estimation(self): """ Calculates the extrinsic calibration from the coordinate space of the camera to the coordinate space spanned by a chessboard by retrieving the 3d coordinates of the chessboard with the depth information and subsequently using the kabsch algortihm for finding the optimal rigid transformation between the two coordinate spaces Returns: ----------- retval : dict keys: str Serial number of the device values: [success, transformation, points2D, rmsd] success: bool transformation: Transformation Rigid transformation from the coordinate system of the camera to the coordinate system of the chessboard points2D: array [2,N] array of the chessboard corners used for pose_estimation rmsd: Root mean square deviation between the observed chessboard corners and the corners in the local coordinate system after transformation """ corners3D = self.get_chessboard_corners_in3d() retval = {} for (serial, [found_corners, points2D, points3D, validPoints] ) in corners3D.items(): objectpoints = get_chessboard_points_3D(self.chessboard_params) retval[serial] = [False, None, None, None] if found_corners == True: #initial vectors are just for correct dimension valid_object_points = objectpoints[:,validPoints] valid_observed_object_points = points3D[:,validPoints] #check for sufficient points if valid_object_points.shape[1] < 5: print("Not enough points have a valid depth for calculating the transformation") else: [rotation_matrix, translation_vector, rmsd_value] = calculate_transformation_kabsch(valid_object_points, valid_observed_object_points) retval[serial] =[True, Transformation(rotation_matrix, translation_vector), points2D, rmsd_value] print("RMS error for calibration with device number", serial, "is :", rmsd_value, "m") return retval def find_chessboard_boundary_for_depth_image(self): boundary = {} for (info, frameset) in self.frames.items(): serial = info[0] product_line = info[1] depth_frame = post_process_depth_frame(frameset[rs.stream.depth]) infrared_frame = frameset[(rs.stream.infrared, 1)] found_corners, points2D = cv_find_chessboard(depth_frame, infrared_frame, self.chessboard_params) boundary[serial] = [np.floor(np.amin(points2D[0,:])).astype(int), np.floor(np.amax(points2D[0,:])).astype(int), np.floor(np.amin(points2D[1,:])).astype(int), np.floor(np.amax(points2D[1,:])).astype(int)] return boundary