refine_extrinsic_parameter_fgr.py

import sys
import re
import os
import glob
import shutil
import copy

try :
    import cv2
except :
    sys.path.remove("/opt/ros/kinetic/lib/python2.7/dist-packages")
    import cv2
import numpy as np
from open3d import *
import scipy as sp
from scipy.spatial.transform import Rotation as Rot
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

from functools import partial
import time
import toml

toml_params = toml.load(open('../cfg/recognition_parameter.toml'))

DATA_SAVE_DIR = toml_params["Parameter"]["data_save_dir"]
PCD_DIR_LEFT = DATA_SAVE_DIR +  "/pcd/left"
PCD_DIR_RIGHT  = DATA_SAVE_DIR + "/pcd/right"

### drawing functions
def draw_hist(x):
    fig = plt.figure()
    ax = fig.add_subplot(1,1,1)
    ax.hist(x, bins=50)
    ax.set_title('histogram')
    ax.set_xlabel('x')
    ax.set_ylabel('freq')
    ax.axvline(np.median(x), color='red', linewidth=1)
    fig.show()

def draw_before_registration(source, target):
    source_temp = copy.deepcopy(source)
    target_temp = copy.deepcopy(target)
    source_temp.paint_uniform_color([1, 0.706, 0])
    target_temp.paint_uniform_color([0, 0.651, 0.929])
    draw_geometries([source_temp, target_temp])

def draw_registration_result(source, target, transformation):
    source_temp = copy.deepcopy(source)
    target_temp = copy.deepcopy(target)
    source_temp.paint_uniform_color([1, 0.706, 0])
    target_temp.paint_uniform_color([0, 0.651, 0.929])
    source_temp.transform(transformation)
    draw_geometries([source_temp, target_temp])

### downsampling + FPFH
def preprocess_point_cloud(pcd, voxel_size):
    print(":: Downsample with a voxel size %.3f." % voxel_size)
    pcd_down = voxel_down_sample(pcd, voxel_size)

    radius_normal = voxel_size * 2
    print(":: Estimate normal with search radius %.3f." % radius_normal)
    estimate_normals(
        pcd_down,
        KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30))

    radius_feature = voxel_size * 5
    print(":: Compute FPFH feature with search radius %.3f." % radius_feature)
    pcd_fpfh = compute_fpfh_feature(
        pcd_down,
        KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100))
    return pcd_down, pcd_fpfh

def prepare_dataset(source, voxel_size):
    source_down, source_fpfh = preprocess_point_cloud(source, voxel_size)
    return source, source_down,source_fpfh

### global matching
def execute_fast_global_registration(source_down, target_down, source_fpfh,
                                     target_fpfh, voxel_size):
    distance_threshold = voxel_size * 0.5
    print(":: Apply fast global registration with distance threshold %.3f" \
            % distance_threshold)
    result = registration_fast_based_on_feature_matching(
        source_down, target_down, source_fpfh, target_fpfh,
        FastGlobalRegistrationOption(
            maximum_correspondence_distance=distance_threshold))
    return result

left_pcd_data = glob.glob(PCD_DIR_LEFT + "/*.pcd")
#left_pcd_data = glob.glob(PCD_DIR_LEFT + "/0005.pcd")
left_pcd_data = np.sort(left_pcd_data)
right_pcd_data = glob.glob(PCD_DIR_RIGHT + "/*.pcd")
#right_pcd_data = glob.glob(PCD_DIR_RIGHT + "/0005.pcd")
right_pcd_data = np.sort(right_pcd_data)

quaternion_list = []
translation_list = []
scale_list = []
err_list = []

### initialize extrinsic parameters
translation_x = toml_params["CameraParameter"]["translation_x"]
translation_y = toml_params["CameraParameter"]["translation_y"]
translation_z = toml_params["CameraParameter"]["translation_z"]
rot_angle_roll = toml_params["CameraParameter"]["rot_angle_roll"]
rot_angle_pitch = toml_params["CameraParameter"]["rot_angle_pitch"]
rot_angle_yaw = toml_params["CameraParameter"]["rot_angle_yaw"]

r = Rot.from_euler('yzx',np.array([rot_angle_yaw, rot_angle_roll, rot_angle_pitch]),degrees=True) 
rot_mat_init =  r.as_dcm()
degrees_init = r.as_euler('yzx',degrees=True) 
translation_init = np.array([translation_x,translation_y,translation_z])

degrees_list = []
translation_list = []

for i in np.arange(len(left_pcd_data)):
#for i in np.arange(30):
    left_pcd = read_point_cloud(left_pcd_data[i])
    _right_pcd = read_point_cloud(right_pcd_data[i])

    # transformation point clouds using configured extrinsic parameters
    right_pcd = PointCloud()
    right_pcd.points = Vector3dVector(np.asarray(_right_pcd.points).dot(rot_mat_init.T) + translation_init) 

    # down sampling + feature extraction
    pcd_left, pcd_left_fltd, fpfh_left = prepare_dataset(left_pcd, 0.002)
    pcd_right, pcd_right_fltd, fpfh_right = prepare_dataset(right_pcd, 0.002)

    # run global matching
    result_fgr = execute_fast_global_registration(pcd_right_fltd, pcd_left_fltd, fpfh_right, fpfh_left, 0.002)
    print(i)
    print(result_fgr)
    #draw_registration_result(pcd_right_fltd, pcd_left_fltd, result_fgr.transformation)
    #draw_before_registration(pcd_right_fltd, pcd_left_fltd)

    #rotation mat -> degrees (for statistical processing)
    r_res = Rot.from_dcm(result_fgr.transformation[:3,:3])
    _degrees_res = r_res.as_euler('yxz',degrees=True)

    #store result
    _translation_res = result_fgr.transformation[:3,3]    
    degrees_list.append(_degrees_res)
    translation_list.append(_translation_res)

# statistical processing
degrees_list_ary = np.asarray(degrees_list)
translation_list_ary = np.asarray(translation_list)
_degrees_res = np.median(degrees_list_ary,axis=0)
_translation_res = np.median(translation_list_ary,axis=0)

degrees_res = degrees_init + _degrees_res
translation_res = translation_init + _translation_res

print(" ")
print("<extrinsic parameters>")
print("rot_angle_roll = %f"%(degrees_res[2]))
print("rot_angle_pitch = %f"%(degrees_res[1]))
print("rot_angle_yaw = %f"%(degrees_res[0]))
print("translation_x = %f"%(translation_res[0]))
print("translation_y = %f"%(translation_res[1]))
print("translation_z = %f"%(translation_res[2]))