calibration_kabsch.py

Go to the documentation of this file.

 
 
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