def marker_registration(source, target):
cad_src, depth_src = source
cad_des, depth_des = target
gray_src = cv2.cvtColor(cad_src, cv2.COLOR_RGB2GRAY)
gray_des = cv2.cvtColor(cad_des, cv2.COLOR_RGB2GRAY)
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
parameters = aruco.DetectorParameters_create()
#lists of ids and the corners beloning to each id
corners_src, _ids_src, rejectedImgPoints = aruco.detectMarkers(
gray_src, aruco_dict, parameters=parameters)
corners_des, _ids_des, rejectedImgPoints = aruco.detectMarkers(
gray_des, aruco_dict, parameters=parameters)
try:
ids_src = []
ids_des = []
for i in range(len(_ids_src)):
ids_src.append(_ids_src[i][0])
for i in range(len(_ids_des)):
ids_des.append(_ids_des[i][0])
except:
return None
common = [x for x in ids_src if x in ids_des]
if len(common) < 2:
# too few marker matches, use icp instead
return None
src_good = []
dst_good = []
for i, id in enumerate(ids_des):
if id in ids_src:
j = ids_src.index(id)
for count, corner in enumerate(corners_src[j][0]):
feature_3D_src = depth_src[int(corner[1])][int(corner[0])]
feature_3D_des = depth_des[int(
corners_des[i][0][count][1])][int(
corners_des[i][0][count][0])]
if feature_3D_src[2] != 0 and feature_3D_des[2] != 0:
src_good.append(feature_3D_src)
dst_good.append(feature_3D_des)
# get rigid transforms between 2 set of feature points through ransac
try:
transform = match_ransac(np.asarray(src_good), np.asarray(dst_good))
return transform
except:
return None
if '/opt/ros/kinetic/lib/python2.7/dist-packages' in sys.path:
sys.path.remove('/opt/ros/kinetic/lib/python2.7/dist-packages')
from registration import icp, feature_registration, match_ransac, rigid_transform_3D
import numpy as np
from scipy.linalg import svd
start_points = np.array([[-0.1, -0.1, 0.], [-0.1, -0.1, 0.3], [-0.1, 0.1, 0.],
[-0.1, 0.1, 0.3], [0.1, -0.1, 0.], [0.1, -0.1, 0.3],
[0.1, 0.1, 0.], [0.1, 0.1, 0.3]])
end_points = np.array([[-0.15, -0.1, 0.], [0.15, -0.1, 0.], [-0.15, -0.1, 0.2],
[0.15, -0.1, 0.2], [-0.15, 0.1, 0.], [0.15, 0.1, 0.],
[-0.15, 0.1, 0.2], [0.15, 0.1, 0.2]])
try:
transform = match_ransac(np.asarray(start_points), np.asarray(end_points))
print(transform)
except:
print('no')
m = start_points.shape[0]
transform = np.array(transform)
h**o = np.ones((m, 4))
h**o[:, :3] = start_points
for i in range(m):
point = h**o[i]
tmp = transform.dot(point.T)
print(tmp / tmp[3])
print('-----------------------')
