7 import pyrealsense2
as rs
8 import calculate_rmsd_kabsch
as rmsd
10 from helper_functions
import cv_find_chessboard, get_chessboard_points_3D, get_depth_at_pixel, convert_depth_pixel_to_metric_coordinate
11 from realsense_device_manager
import post_process_depth_frame
15 | | | | ___ | | _ __ ___ _ __ | ___|_ _ _ __ ___ | |_ (_) ___ _ __ ___ 16 | |_| | / _ \| || '_ \ / _ \| '__| | |_ | | | || '_ \ / __|| __|| | / _ \ | '_ \ / __| 17 | _ || __/| || |_) || __/| | | _| | |_| || | | || (__ | |_ | || (_) || | | |\__ \ 18 |_| |_| \___||_|| .__/ \___||_| |_| \__,_||_| |_| \___| \__||_| \___/ |_| |_||___/ 25 Calculates the optimal rigid transformation from src_points to 27 (regarding the least squares error) 38 rotation_matrix: array 41 translation_vector: array 47 assert src_points.shape == dst_points.shape
48 if src_points.shape[0] != 3:
49 raise Exception(
"The input data matrix had to be transposed in order to compute transformation.")
51 src_points = src_points.transpose()
52 dst_points = dst_points.transpose()
54 src_points_centered = src_points - rmsd.centroid(src_points)
55 dst_points_centered = dst_points - rmsd.centroid(dst_points)
57 rotation_matrix = rmsd.kabsch(src_points_centered, dst_points_centered)
58 rmsd_value = rmsd.kabsch_rmsd(src_points_centered, dst_points_centered)
60 translation_vector = rmsd.centroid(dst_points) - np.matmul(rmsd.centroid(src_points), rotation_matrix)
62 return rotation_matrix.transpose(), translation_vector.transpose(), rmsd_value
68 | \/ | __ _ (_) _ __ / ___| ___ _ __ | |_ ___ _ __ | |_ 69 | |\/| | / _` || || '_ \ | | / _ \ | '_ \ | __|/ _ \| '_ \ | __| 70 | | | || (_| || || | | | | |___| (_) || | | || |_| __/| | | || |_ 71 |_| |_| \__,_||_||_| |_| \____|\___/ |_| |_| \__|\___||_| |_| \__| 76 def __init__(self, rotation_matrix, translation_vector):
78 self.
pose_mat[:3,:3] = rotation_matrix
79 self.
pose_mat[:3,3] = translation_vector.flatten()
84 Applies the transformation to the pointcloud 89 (3, N) matrix where N is the number of points 93 points_transformed : array 94 (3, N) transformed matrix 96 assert(points.shape[0] == 3)
98 points_ = np.vstack((points, np.ones((1,n))))
99 points_trans_ = np.matmul(self.
pose_mat, points_)
100 points_transformed = np.true_divide(points_trans_[:3,:], points_trans_[[-1], :])
101 return points_transformed
105 Computes the inverse transformation and returns a new Transformation object 109 inverse: Transformation 112 rotation_matrix = self.
pose_mat[:3,:3]
113 translation_vector = self.
pose_mat[:3,3]
115 rot = np.transpose(rotation_matrix)
116 trans = - np.matmul(np.transpose(rotation_matrix), translation_vector)
123 def __init__(self, frames, intrinsic, chessboard_params):
124 assert(len(chessboard_params) == 3)
131 Searches the chessboard corners in the infrared images of 132 every connected device and uses the information in the 133 corresponding depth image to calculate the 3d 134 coordinates of the chessboard corners in the coordinate system of 141 Serial number of the device 142 values: [success, points3D, validDepths] 144 Indicates wether the operation was successfull 146 (3,N) matrix with the coordinates of the chessboard corners 147 in the coordinate system of the camera. N is the number of corners 148 in the chessboard. May contain points with invalid depth values 150 Sequence with length N indicating which point in points3D has a valid depth value 153 for (info, frameset)
in self.
frames.items():
155 product_line = info[1]
157 if product_line ==
"L500":
158 infrared_frame = frameset[(rs.stream.infrared, 0)]
160 infrared_frame = frameset[(rs.stream.infrared, 1)]
161 depth_intrinsics = self.
intrinsic[serial][rs.stream.depth]
163 corners3D[serial] = [found_corners,
None,
None,
None]
165 points3D = np.zeros((3, len(points2D[0])))
166 validPoints = [
False] * len(points2D[0])
167 for index
in range(len(points2D[0])):
168 corner = points2D[:,index].flatten()
170 if depth != 0
and depth
is not None:
171 validPoints[index] =
True 173 points3D[0, index] = X
174 points3D[1, index] = Y
175 points3D[2, index] = Z
176 corners3D[serial] = found_corners, points2D, points3D, validPoints
182 Calculates the extrinsic calibration from the coordinate space of the camera to the 183 coordinate space spanned by a chessboard by retrieving the 3d coordinates of the 184 chessboard with the depth information and subsequently using the kabsch algortihm 185 for finding the optimal rigid transformation between the two coordinate spaces 191 Serial number of the device 192 values: [success, transformation, points2D, rmsd] 194 transformation: Transformation 195 Rigid transformation from the coordinate system of the camera to 196 the coordinate system of the chessboard 198 [2,N] array of the chessboard corners used for pose_estimation 200 Root mean square deviation between the observed chessboard corners and 201 the corners in the local coordinate system after transformation 205 for (serial, [found_corners, points2D, points3D, validPoints] )
in corners3D.items():
207 retval[serial] = [
False,
None,
None,
None]
208 if found_corners ==
True:
210 valid_object_points = objectpoints[:,validPoints]
211 valid_observed_object_points = points3D[:,validPoints]
214 if valid_object_points.shape[1] < 5:
215 print(
"Not enough points have a valid depth for calculating the transformation")
219 retval[serial] =[
True,
Transformation(rotation_matrix, translation_vector), points2D, rmsd_value]
220 print(
"RMS error for calibration with device number", serial,
"is :", rmsd_value,
"m")
227 for (info, frameset)
in self.
frames.items():
229 product_line = info[1]
231 if product_line ==
"L500":
232 infrared_frame = frameset[(rs.stream.infrared, 0)]
234 infrared_frame = frameset[(rs.stream.infrared, 1)]
236 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)]
def calculate_transformation_kabsch(src_points, dst_points)
def cv_find_chessboard(depth_frame, infrared_frame, chessboard_params)
def __init__(self, frames, intrinsic, chessboard_params)
def perform_pose_estimation(self)
def convert_depth_pixel_to_metric_coordinate(depth, pixel_x, pixel_y, camera_intrinsics)
def get_chessboard_points_3D(chessboard_params)
def get_chessboard_corners_in3d(self)
def find_chessboard_boundary_for_depth_image(self)
static std::string print(const transformation &tf)
def get_depth_at_pixel(depth_frame, pixel_x, pixel_y)
def post_process_depth_frame(depth_frame, decimation_magnitude=1.0, spatial_magnitude=2.0, spatial_smooth_alpha=0.5, spatial_smooth_delta=20, temporal_smooth_alpha=0.4, temporal_smooth_delta=20)