def get_transformed_centroids(self, reference_centroids=None):
def get_transformed_tumor_centroids(gtvs, transform):
#finds the mean organ centroid locations and applies an affine transform
#to each of the centroids in the current dataset
#returns a transformed 3d centroid matrix and a list of arrays with new tumor centroid positions
nonzero_centroids = [g.position for g in gtvs if g.volume > 0]
t_centroids = np.ones((len(nonzero_centroids), 4))
pos = 0
for gtv in nonzero_centroids:
t_centroids[pos, 0:3] = gtv
pos += 1
new_centroids = np.dot(transform, t_centroids.T).T
return new_centroids
if reference_centroids is None:
reference_centroids = self.centroids.mean(axis=0)
transformed_tumor_centroids = []
transformed_organ_centroids = np.empty(self.centroids.shape)
for p in range(self.get_num_patients()):
centroids = self.centroids[p, :, :]
transform = estimateAffine3D(centroids, reference_centroids)[1]
ref = np.hstack([centroids, np.ones((Constants.num_organs, 1))])
transformed_centroids = np.dot(transform, ref.T)
transformed_organ_centroids[p, :, :] = transformed_centroids.T
p_tumor_centroids = get_transformed_tumor_centroids(
self.gtvs[p], transform)
transformed_tumor_centroids.append(p_tumor_centroids)
return transformed_organ_centroids, transformed_tumor_centroids
def estimateRestrictedAffineTransform(source: np.array,
target: np.array,
verbose=False):
SourceHom = np.transpose(np.hstack([source,
np.ones([source.shape[0], 1])]))
TargetHom = np.transpose(np.hstack([target,
np.ones([source.shape[0], 1])]))
RetVal, AffineTrans, Inliers = cv2.estimateAffine3D(source, target)
# We assume no shear in the affine matrix and decompose into rotation, non-uniform scales, and translation
Translation = AffineTrans[:3, 3]
NUScaleRotMat = AffineTrans[:3, :3]
# NUScaleRotMat should be the matrix SR, where S is a diagonal scale matrix and R is the rotation matrix (equivalently RS)
# Let us do the SVD of NUScaleRotMat to obtain R1*S*R2 and then R = R1 * R2
R1, ScalesSorted, R2 = np.linalg.svd(NUScaleRotMat, full_matrices=True)
if verbose:
print(
'-----------------------------------------------------------------------'
)
# Now, the scales are sort in ascending order which is painful because we don't know the x, y, z scales
# Let's figure that out by evaluating all 6 possible permutations of the scales
ScalePermutations = list(itertools.permutations(ScalesSorted))
MinResidual = 1e8
Scales = ScalePermutations[0]
OutTransform = np.identity(4)
Rotation = np.identity(3)
for ScaleCand in ScalePermutations:
CurrScale = np.asarray(ScaleCand)
CurrTransform = np.identity(4)
CurrRotation = (np.diag(1 / CurrScale) @ NUScaleRotMat).transpose()
CurrTransform[:3, :3] = np.diag(CurrScale) @ CurrRotation
CurrTransform[:3, 3] = Translation
# Residual = evaluateModel(CurrTransform, SourceHom, TargetHom)
Residual = evaluateModelNonHom(source, target, CurrScale, CurrRotation,
Translation)
if verbose:
# print('CurrTransform:\n', CurrTransform)
print('CurrScale:', CurrScale)
print('Residual:', Residual)
print('AltRes:',
evaluateModelNoThresh(CurrTransform, SourceHom, TargetHom))
if Residual < MinResidual:
MinResidual = Residual
Scales = CurrScale
Rotation = CurrRotation
OutTransform = CurrTransform
if verbose:
print('Best Scale:', Scales)
if verbose:
print('Affine Scales:', Scales)
print('Affine Translation:', Translation)
print('Affine Rotation:\n', Rotation)
print(
'-----------------------------------------------------------------------'
)
return Scales, Rotation, Translation, OutTransform
def bounding_box_transform(set1, set2):
src = np.array([set1], copy=True).astype(np.float32)
dst = np.array([set2], copy=True).astype(np.float32)
t = cv2.estimateAffine3D(src, dst, confidence=0.99)
return t[1]
def ransac_align_points(
pA, pB, threshold, diagonal_constraint=0.75, default=np.eye(4)[:3],
):
"""
"""
# sensible requirement of 51 or more spots to compute ransac affine
if len(pA) <= 50 or len(pB) <= 50:
if default is not None:
print("Insufficient spot matches for ransac, returning default identity")
return default
else:
raise ValueError("Insufficient spot matches for ransac, need more than 50")
# compute the affine
r, Aff, inline = cv2.estimateAffine3D(pA, pB, ransacThreshold=threshold, confidence=0.999)
# rarely ransac just doesn't work (depends on data and parameters)
# sensible choices for hard constraints on the affine matrix
if np.any( np.diag(Aff) < diagonal_constraint ):
if default is not None:
print("Degenerate affine produced, returning default identity")
return default
else:
raise ValueError("Degenerate affine produced, ransac failed")
return Aff
def register_icp(self, max_iter=20, tolerance=1e-4):
"""
point to point matching using interative closest points
:param max_iter: maximum number of iteration
:param tolerance: tolerance to stop algorithm
:return:
"""
point_a = self.moving_points
point_b = self.fixed_points
pre_rmse = 0
pvmse = None
for it in range(max_iter):
# find the correspondence
nbrs = NearestNeighbors(n_neighbors=1, n_jobs=-1).fit(point_b)
dist, ind = nbrs.kneighbors(point_a)
# estimate transformation given correspondence
point_c = point_b[ind.flatten()]
# point_c = point_c - np.mean(point_c, axis=0)
# point_a = point_a - np.mean(point_a, axis=0)
# f = plt.figure()
# ax = Axes3D(f)
# ax.scatter(point_a[:, 0], point_a[:, 1], point_a[:, 2], c='g')
# ax.scatter(point_c[:, 0], point_c[:, 1], point_c[:, 2], c='r')
# plt.show()
# Update the transformation
M = cv2.estimateAffine3D(point_a, point_c)
# compute new location
point_a = np.hstack((point_a, np.ones((point_a.shape[0], 1))))
point_a = np.dot(point_a, M[1].transpose())
# f = plt.figure()
# ax = Axes3D(f)
# ax.scatter(point_c[:, 0], point_c[:, 1], point_c[:, 2], c='r')
# ax.scatter(point_a[:, 0], point_a[:, 1], point_a[:, 2], c='g')
# plt.show()
# print('debug')
# compute rmse
err = np.sum(np.square(point_a - point_c), axis=1)
rmse = np.sqrt(np.sum(err) / len(err))
if abs(rmse - pre_rmse) < tolerance:
pre_rmse = rmse
pvmse = err / 3
break
else:
pre_rmse = rmse
pvmse = err / 3
# print(rmse)
return pvmse, pre_rmse
def createTransformFunc(ptPairs, direction):
"""
Returns a function that takes a point (x,y,z) of a camera coordinate, and returns the robot (x,y,z) for that point
Direction is either "toRob" or "toCam".
If "toCam" it will return a Robot-->Camera transform
If "toRob" it will return a Camera-->Robot transform
"""
ptPairArray = np.asarray(ptPairs)
if direction == "toRob":
pts1 = ptPairArray[:, 0]
pts2 = ptPairArray[:, 1]
if direction == "toCam":
pts1 = ptPairArray[:, 1]
pts2 = ptPairArray[:, 0]
# Generate the transformation matrix
ret, M, mask = cv2.estimateAffine3D(np.float32(pts1),
np.float32(pts2),
confidence = .9999999)
if not ret: printf("RobotVision| ERROR: Transform failed!")
# Returns a function that will perform the transformation between pointset 1 and pointset 2 (if direction is == 1)
transformFunc = lambda x: np.array((M * np.vstack((np.matrix(x).reshape(3, 1), 1)))[0:3, :].reshape(1, 3))[0]
"""
Breakdown of function. Here's an example of transforming [95, -35, 530] cam which is [5, 15, 15] in the robot grid
First:
[95, -35, 530]
np.vstack((np.matrix(input).reshape(3, 1), 1)) is applied, converts it to a vertical array
[[ 95]
[-35]
[530]
[ 1]]
M * np.vstack((np.matrix(x).reshape(3, 1), 1)) (transformation matrix multiplied by the previous number)
[[ 5.8371337 ]
[ 15.72722685]
[ 14.5007519 ]]
There you go. The rest of it is simply converting that matrix into
[5.83, 15.72, 14.5]
but you know, without the rounding.
"""
return transformFunc
def test_estimateAffine3D(self):
pattern_size = (11, 8)
pattern_points = np.zeros((np.prod(pattern_size), 3), np.float32)
pattern_points[:,:2] = np.indices(pattern_size).T.reshape(-1, 2)
pattern_points *= 10
(retval, out, inliers) = cv2.estimateAffine3D(pattern_points, pattern_points)
self.assertEqual(retval, 1)
if cv2.norm(out[2,:]) < 1e-3:
out[2,2]=1
self.assertLess(cv2.norm(out, np.float64([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0]])), 1e-3)
self.assertEqual(cv2.countNonZero(inliers), pattern_size[0]*pattern_size[1])
def test_estimateAffine3D(self):
pattern_size = (11, 8)
pattern_points = np.zeros((np.prod(pattern_size), 3), np.float32)
pattern_points[:,:2] = np.indices(pattern_size).T.reshape(-1, 2)
pattern_points *= 10
(retval, out, inliers) = cv2.estimateAffine3D(pattern_points, pattern_points)
self.assertEqual(retval, 1)
if cv2.norm(out[2,:]) < 1e-3:
out[2,2]=1
self.assertLess(cv2.norm(out, np.float64([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0]])), 1e-3)
self.assertEqual(cv2.countNonZero(inliers), pattern_size[0]*pattern_size[1])
def estimate_extrinsics(dataset):
"""
Estimate Extrinsic parameters from world to cam point correspondences
:param dataset:
:return:
"""
# extrinsics are matrices M of shape (3,4) for every datapoint --> M = [R,t] where R=rotation matrix and t = translation vector
camera_extrinsics_univ = np.zeros(
(dataset.datadict["keypoints_3d_univ"].shape[0], 3, 4), dtype=np.float)
camera_extrinsics = np.zeros(
(dataset.datadict["keypoints_3d"].shape[0], 3, 4), dtype=np.float)
for i, vid in enumerate(
tqdm(
np.unique(dataset.datadict["v_ids"]),
desc="Estimate extrinsics per video",
)):
ids = dataset.datadict["v_ids"] == vid
kps3d_c = dataset.datadict["keypoints_3d"][ids]
kps3d_c_univ = dataset.datadict["keypoints_3d_univ"][ids]
kps3d_w = dataset.datadict["keypoints_3d_world"][ids]
kps3d_c = np.reshape(kps3d_c, (-1, 3))
kps3d_c_univ = np.reshape(kps3d_c_univ, (-1, 3))
kps3d_w = np.reshape(kps3d_w, (-1, 3))
_, M, _ = cv2.estimateAffine3D(kps3d_w,
kps3d_c,
ransacThreshold=10,
confidence=0.999)
_, M_univ, _ = cv2.estimateAffine3D(kps3d_w,
kps3d_c_univ,
ransacThreshold=10,
confidence=0.999)
# returned values correspond to [R,t]^T
camera_extrinsics[ids] = M
camera_extrinsics_univ[ids] = M_univ
return camera_extrinsics_univ, camera_extrinsics
def fine_reg_cv2(self, num_iter=40):
trans_mat_set = []
error_set = []
for i in xrange(len(self.R_set)):
# initial pose
R = self.R_set[i]
t = self.t_set[i]
trans_mat = np.concatenate((np.concatenate(
(R, t), axis=1), np.array([[0, 0, 0, 1]])),
axis=0)
print 'initial transformation', trans_mat
src_pts = (trans_mat[:3, 0:3].dot(self.cam_pts.T) +
trans_mat[:3, 3:4]).T.astype(np.float32)
dst_pts = self.all_lidar_pts.astype(np.float32)
squared_error = 0
for j in xrange(num_iter):
(distances, idx) = self.lidar_nn.query(src_pts,
k=1,
return_distance=True)
squared_error = np.square(distances).sum()
print "iter no. %d squared error %f" % (j, squared_error)
retval, trans_mat_delta, inliers = cv2.estimateAffine3D(
src_pts, dst_pts[idx.T][0], ransacThreshold=0.5)
#trans_mat_delta = np.concatenate((trans_mat_delta, np.array([[0, 0, 0, 1]])), axis=0)
idx = idx[np.nonzero(inliers)[0]]
trans_mat_delta = self.svd_transform(
src_pts[np.nonzero(inliers)[0]], dst_pts[idx.T][0])
print 'number of inliers', inliers.sum()
# update transformation matrix and src points
src_pts = (trans_mat_delta[:3, 0:3].dot(src_pts.T) +
trans_mat_delta[:3, 3:4]).T
trans_mat = trans_mat_delta.dot(trans_mat)
trans_mat_set.append(trans_mat)
error_set.append(squared_error)
low_error = float('Inf')
optimal_mat = trans_mat_set[0]
for i in xrange(len(trans_mat_set)):
if low_error > error_set[i]:
low_error = error_set[i]
optimal_mat = trans_mat_set[0]
print optimal_mat
print low_error
self.trans_mat_set = trans_mat_set
self.error_set = error_set
def CameraToArm_Affine_Transformation(data):
"""Get affine transformation matrix"""
Arm_Coordinates = data[:, [0, 1, 2]]
Camera_Coordinates = data[:, [3, 4, 5]]
retval, affine, inlier = cv.estimateAffine3D(Arm_Coordinates,
Camera_Coordinates)
try:
if retval == 1:
rotation = affine[:, [0, 1, 2]]
translation = affine[:, [3]]
return rotation, translation
except ValueError:
print("Calibration Points not correct. Please try again")
pass
def ransac_align_points(
pA,
pB,
threshold,
diagonal_constraint=0.75,
default=np.eye(4),
):
"""
"""
# sensible requirement of 50 or more spots to compute ransac affine
if len(pA) < 50 or len(pB) < 50:
if default is not None:
print("Insufficient spot matches for ransac")
print("Returning default")
return default
else:
message = "Insufficient spot matches for ransac"
message += ", need 50 or more"
raise ValueError(message)
# compute the affine
r, Aff, inline = cv2.estimateAffine3D(
pA,
pB,
ransacThreshold=threshold,
confidence=0.999,
)
# rarely ransac just doesn't work (depends on data and parameters)
# sensible choices for hard constraints on the affine matrix
if np.any(np.diag(Aff) < diagonal_constraint):
if default is not None:
print("Degenerate affine produced")
print("Returning default")
return default
else:
message = "Degenerate affine produced"
message += ", ransac failed"
raise ValueError(message)
# augment affine to 4x4 matrix
affine = np.eye(4)
affine[:3, :] = Aff
return affine
def __generateTransform(self, fromPts, toPts):
"""
This will generate a function that will work like this function(x1, y1, z1) and returns (x2, y2, z2), where
x1, y1, z1 are form the "fromPts" coordinate grid, and the x2, y2, z2 are from the "toPts" coordinate grid.
It does this by using openCV's estimateAffine3D
"""
# Generate the transformation matrix
ret, M, mask = cv2.estimateAffine3D(np.float32(fromPts),
np.float32(toPts),
confidence = .9999999)
if not ret: printf("RobotVision| ERROR: Transform failed!")
transformFunc = lambda x: np.array((M * np.vstack((np.matrix(x).reshape(3, 1), 1)))[0:3, :].reshape(1, 3))[0]
return transformFunc
def estimate_camera_to_plane(plane, pts, p0, p1):
"""estimate an affine transform from camera to plane
plane - 4x1 3D plane equation
pts - Nx3 points on the plane in camera coordinates
p0 - the new origin in world coordinates
"""
# TODO: maybe, select this point in a smarter way
a = (p1 - p0)
a /= la.norm(a)
b = np.cross(a, plane[:3])
b /= la.norm(b)
# projecting the points to the new basis
proj_pts = np.inner(pts - p0, [a, b, plane[:3]])
ret, T, val = cv2.estimateAffine3D(pts, proj_pts, confidence=1.0)
return np.vstack((T, [0, 0, 0, 1]))
def register_point_set(self, point_set_1: np.ndarray,
point_set_2: np.ndarray
) -> Tuple[np.ndarray, np.ndarray]:
"""Find optimal affine transformation between the points sets
function used:
https://docs.opencv.org/4.0.0/d9/d0c/group__calib3d.html#ga396afb6411b30770e56ab69548724715
Args:
point_set_1 (np.ndarray): 3xn
point_set_2 (np.ndarray): 3xn
Returns:
np.ndarray: returns R (3x3 rot matrix), t (3x1 matrix)
"""
# Input: expects 3xN matrix of points
# Returns R,t
# R = 3x3 rotation matrix
# t = 3x1 column vector
A = self._check_and_adjust_dimension(point_set_1)
B = self._check_and_adjust_dimension(point_set_2)
A = A.T
logger.debug(f"\n {A}")
B = B.T
logger.debug(f"\n {B}")
retval, out, inlier = cv2.estimateAffine3D(src=A,
dst=B,
ransacThreshold=self._threshold,
confidence=self._confidence,
)
logger.debug(f"Number of point correspondances: {A.shape[0]}")
logger.debug(
f"Number of inlier: {sum([1 if x == 1 else 0 for x in inlier])}")
logger.debug(f"""
Parameters used for ransac:
Threshold {self._threshold}
CONFIDENCE {self._confidence}
""")
logger.info(f"Result: \n {out}")
return out[:, :3], out[:, 3]
def estimate_transformation(width,height,points,image_folder,camera_matrix,roi,ball_radius):
'''return transformation matrix
arguments:
width,height - for images
points - real points in robot axes
image_foler - folder with image_{}.png depth_{}.png
camera_matrix
roi - rect of interest
ball_radius - in mm
note, that z is the vertical axis, so to calibrate we need to
'''
number_images = len(glob.glob(image_folder+'*.png'))//2
mask = np.zeros((height,width,3))
mask[roi['y']:roi['y']+roi['height'],roi['x']:roi['x']+roi['width'],:]=1.0
camera_coords = [get_world_coords_n(i, mask, camera_matrix, image_folder) for i in range(number_images)]
for i in range(number_images):
d = np.linalg.norm(camera_coords[i])
camera_coords[i] *= 1.0+ ball_radius/d
sphere_center_in_robot_axes = np.zeros((len(points),3))
for i in range(len(points)):
sphere_center_in_robot_axes[i,0] = points[i]["x"]
sphere_center_in_robot_axes[i,1] = points[i]["y"]
sphere_center_in_robot_axes[i,2] = points[i]["z"] - ball_radius #robot grabs a ball in the upper point
camera_coords = np.array(camera_coords).astype(np.float32)
sphere_center_in_robot_axes = np.array(sphere_center_in_robot_axes).astype(np.float32)
#compute calibration
trans = cv2.estimateAffine3D(camera_coords, sphere_center_in_robot_axes)[1]
return trans
def affine_3D_tx_lip_landmarks_src_to_dst(source_frame_2D_landmarks, source_frame_3D_landmarks,
target_frame_2D_landmarks, target_frame_3D_landmarks):
# Estimate Affine 3D transformation between the first 36 landmarks (jaw, eyebrows, eyes, nose bridge, nose base) from source to target
retval, Rt_to_dst_from_src, _ = cv2.estimateAffine3D(source_frame_3D_landmarks[:36], target_frame_3D_landmarks[:36])
if retval:
# 3D rotate the 2D lip landmarks (because 2D lip landmarks are more stable than 3D)
# - Attach the z coordinate of 3D landmarks to 2D landmarks, and add homogeneous coordinate
source_frame_combo_3D_lip_landmarks = np.hstack(( np.array(source_frame_2D_landmarks)[48:68],
np.array(source_frame_3D_landmarks)[48:68, 2].reshape(20, 1),
np.ones((20, 1)) ))
# - Rotate
target_lip_landmarks_tx_from_source = np.dot( Rt_to_dst_from_src, source_frame_combo_3D_lip_landmarks.T ).T.astype('int')
# Normalize, un-normalize to match position of target mouth
_, target_lip_landmarks_ux, target_lip_landmarks_uy, target_lip_landmarks_w = normalize_lip_landmarks(target_frame_2D_landmarks[48:68])
new_target_lip_landmarks = np.round(unnormalize_lip_landmarks(normalize_lip_landmarks(target_lip_landmarks_tx_from_source[:, :2])[0],
target_lip_landmarks_ux, target_lip_landmarks_uy, target_lip_landmarks_w)).astype(int)
else:
new_target_lip_landmarks = None
return new_target_lip_landmarks, Rt_to_dst_from_src
def main():
# arguments parsing
parser = argparse.ArgumentParser()
parser.add_argument(
"--listeners",
default="127.0.0.1:9001",
help=
"The ip:port of the OSC programs listening, different listeners must be separated with ';', if multiple ports are used on one ip, it possible to use this form ip1:port1-1;port1-2;port1-3;ip2:port2-1;port2-2..."
)
parser.add_argument(
"--origin_serial",
default=None,
help="The serial number of the tracker used for origin calibration")
parser.add_argument(
"--real_world_points",
default=None,
help=
"The JSON file containing list of points to use for origin calibration"
)
parser.add_argument(
"--framerate",
type=int,
default=60,
help=
"Expected framerate - used to slow down OSC messages if OSC listener can't handle the framerate"
)
parser.add_argument(
"--openvr_coordinates",
default=False,
action="store_true",
help=
"Use openVR native coordinate system if set, or Unity coordinate system if not set"
)
parser.add_argument("--steam",
default=False,
action="store_true",
help="Open SteamVR when the script is launched")
parser.add_argument(
"--oscpattern",
default="/iss/tracker",
help="The OSC pattern used to send messages, default is '/iss/tracker'"
)
parser.add_argument("--bundles",
default=False,
action="store_true",
help="Send single frame messages as a OSC bundle")
args = parser.parse_args()
# global program variables
listeners = [] # array of OSC clients
origin_serial = args.origin_serial
real_world_points = []
real_world_points_array = []
different_h = False # to check if all points are at the same height
current_point_index = -1
calib_points = []
calib_mat = np.identity(4)
# booleans for keystroke interactions
to_unity_world = True # can be modified by --openvr_coordinates option or 'u' key
set_origin = False # change state with 'o' key
calib_next_point = False
escape = False # change state with 'escape' key
# filter used to smooth framerate computation for display
t0_list = []
t0_list_max_size = args.framerate / 2
# array of ids of currently tracked trackers (used to identify newly tracked trackers)
trackers_list = []
print("======================================")
print("")
print("---------------------------------------")
print(" Coordinate system and calibration ")
print("---------------------------------------")
# if --openvr_coordinates option is used, compute coordinates for openvr coordinate system
# if not, compute for Unity coordinate system
to_unity_world = not args.openvr_coordinates
coordsys = "Unity"
if not to_unity_world:
coordsys = "openVR"
print("Coordinate system is set for {0}".format(coordsys))
# load last origin matrix from origin_mat.rtm file,
# the file must be located in the same folder as this script
origin_file_path = os.path.dirname(
os.path.abspath(__file__)) + "\origin_mat.rtm"
#print("try to open origin file at {0}".format(origin_file_path))
origin_file = Path(origin_file_path)
if origin_file.exists():
with open(origin_file_path, 'rb') as loaded_file:
depickler = pickle.Unpickler(loaded_file)
calib_mat = depickler.load()
print("origin loaded from file " + origin_file_path)
#print(calib_mat)
else:
print(
"origin file not found at " + origin_file_path +
", please place the reference and press 'o' key to set a new origin"
)
if origin_serial:
print("Calibration can be done with tracker serial " + origin_serial)
if args.real_world_points:
fp = open(args.real_world_points, 'r')
real_world_points = json.load(fp)
if len(real_world_points) < 4:
real_world_points = None
print("Calibration file must contain at least 4 points")
else:
print("Load real world points from JSON file for calibration")
real_world_points_array = np.zeros((len(real_world_points), 3),
np.float32)
different_h = False
last_h = None
for i, pt in enumerate(real_world_points):
if to_unity_world:
real_world_points_array[i][0] = -pt['x']
real_world_points_array[i][1] = pt['z']
real_world_points_array[i][2] = -pt['y']
else:
real_world_points_array[i][0] = pt['x']
real_world_points_array[i][1] = pt['y']
real_world_points_array[i][2] = pt['z']
# check if there is at least one point outside of floor plan
h = real_world_points_array[i][2]
if last_h is None:
last_h = h
if last_h != h:
different_h = True
last_h = h
print("[{0}] : {1:3.2f}, {2:3.2f}, {3:3.2f}".format(
pt['id'], real_world_points_array[i][0],
real_world_points_array[i][1],
real_world_points_array[i][2]))
# if all points are at the same height,
# create a virtual point and append it to the array
if not different_h:
print(
"All points are at the same height, creating a virtual point for calibration"
)
A = real_world_points_array[0]
B = real_world_points_array[1]
C = real_world_points_array[2]
if collinear(A, B, C):
print(
"ERROR : the 3 first points in real_world_points JSON file must not be aligned"
)
virtual_point = A + np.cross(np.subtract(B, A),
np.subtract(C, A))
real_world_points_array = np.vstack(
[real_world_points_array, virtual_point])
print("[{0}] : {1:3.2f}, {2:3.2f}, {3:3.2f}".format(
'virtual point', virtual_point[0], virtual_point[1],
virtual_point[2]))
print("")
print("---------------------------------------")
print(" OSC parameters ")
print("---------------------------------------")
# parse the ip and ports from listeners and create OSC clients
listeners_str = args.listeners.split(";")
overall_ip = "127.0.0.1"
for l in listeners_str:
listener = l.split(":")
port = -1
if len(listener) == 2:
overall_ip = listener[0]
port = int(listener[1])
elif len(listener) == 1:
port = int(listener[0])
ip = overall_ip
print("Add OSC listener at {0}:{1}".format(ip, port))
listeners.append(udp_client.SimpleUDPClient(ip, port))
print("OSC pattern : " + args.oscpattern)
send_bundles = "Send separate OSC messages for each tracker"
if args.bundles:
send_bundles = "Send simultaneous tracker OSC messages as bundles"
print(send_bundles)
print("")
print("---------------------------------------")
print(" OpenVR ")
print("---------------------------------------")
# init OpenVR
# VRApplication_Other will not open SteamVR
# VRApplication_Scene will open SteamVR
vrapplication_type = openvr.VRApplication_Other
if args.steam:
vrapplication_type = openvr.VRApplication_Scene
print("open SteamVR")
print("Initialize OpenVR ... ", end='')
try:
openvr.init(vrapplication_type)
except openvr.OpenVRError as e:
print("Impossible to intialize OpenVR")
print(e)
exit(0)
vrsystem = openvr.VRSystem()
print("OK")
print("======================================")
print("")
program_t0 = time.perf_counter()
while (not escape):
# keep track of loop time
loop_t0 = time.perf_counter()
t0_list.append(loop_t0)
if len(t0_list) >= t0_list_max_size:
del t0_list[0]
# handle keyboard inputs
key = kbfunc()
if key != 0:
if key == ord('o'):
if origin_serial:
# set up origin
set_origin = True
print("\nset new origin : ")
if real_world_points:
current_point_index = -1
print("Multi-points calibration")
calib_next_point = True
else:
print("Single point calibration")
elif key == ord('n'):
calib_next_point = True
elif key == ord('r'):
print("\nreset origin")
calib_mat = np.identity(4)
elif key == ord('u'):
to_unity_world = not to_unity_world
print("\nto unity world = {0}".format(to_unity_world))
elif key == 27: # escape
escape = True
openvr.shutdown()
# get all devices currently tracked, it include HMD, controllers, lighthouses and trackers
poses_t = openvr.TrackedDevicePose_t * openvr.k_unMaxTrackedDeviceCount
tracked_devices = poses_t()
tracked_devices = vrsystem.getDeviceToAbsoluteTrackingPose(
openvr.TrackingUniverseStanding, 0, len(tracked_devices))
# array to store content that must be sent over OSC
osc_content = []
current_loop_trackers_list = []
for _i, device in enumerate(tracked_devices):
# broswe the array and keey only correctly tracked trackers
if device.bPoseIsValid and vrsystem.getTrackedDeviceClass(
_i) == openvr.TrackedDeviceClass_GenericTracker:
tracker_id = vrsystem.getStringTrackedDeviceProperty(
_i, openvr.Prop_SerialNumber_String)
current_loop_trackers_list.append(tracker_id)
# add tracker_id to list if not already in it
if tracker_id not in trackers_list:
trackers_list.append(tracker_id)
print("\nNew tracker found : {}".format(trackers_list[-1]))
# compute relative position (vector3) and rotation(quaternion) from 3x4 openvr matrix
m_4x4 = vive_pose_to_numpy_matrix_4x4(
device.mDeviceToAbsoluteTracking)
m_corrected = np.matmul(calib_mat, m_4x4)
m_rot = np.identity(3)
for x in range(0, 3):
for y in range(0, 3):
m_rot[x][y] = m_corrected[x][y]
quat = quaternion.from_rotation_matrix(m_rot)
# append computed pos/rot to the list
content = [
tracker_id, m_corrected[0][3], m_corrected[1][3],
m_corrected[2][3], quat.x, quat.y, quat.z, quat.w
]
if to_unity_world:
# switch and invert coordinates if Unity coordinate system is required
content = [
tracker_id, -m_corrected[0][3], -m_corrected[2][3],
m_corrected[1][3], quat.x, quat.z, -quat.y, quat.w
]
osc_content.append(content)
# set new origin if requested
if vrsystem.getStringTrackedDeviceProperty(
_i, openvr.Prop_SerialNumber_String
) == origin_serial and set_origin:
# perform multi-points calibration
if len(real_world_points) >= 4:
if calib_next_point:
if current_point_index < len(
real_world_points) - 1:
print("Place your origin tracker on point")
print(real_world_points[current_point_index +
1])
print("and press 'n'")
if current_point_index < 0:
pass
else:
openvr_pt = m_4x4[0:3, 3]
calib_points.append(openvr_pt)
calib_next_point = False
current_point_index = current_point_index + 1
if current_point_index >= len(real_world_points):
print("Computing calibration with {} points".
format(len(real_world_points)))
calib_points = np.stack(calib_points)
if not different_h:
print(
"All real world points are at same height, creating virtual point"
)
A = calib_points[0]
B = calib_points[1]
C = calib_points[2]
virtual_calib_point = A + np.cross(
np.subtract(B, A), np.subtract(C, A))
calib_points = np.vstack(
[calib_points, virtual_calib_point])
retval, M, inliers = cv2.estimateAffine3D(
calib_points, real_world_points_array)
calib_mat = np.vstack([M, [0, 0, 0, 1]])
with open(origin_file_path,
'wb') as saved_file:
pickler = pickle.Pickler(saved_file)
pickler.dump(calib_mat)
print(calib_mat.round(3))
set_origin = False
# perform single point calibration (e.g, tracker position is origin, rotation matters)
else:
set_origin = False
calib_mat = np.linalg.inv(m_4x4)
with open(origin_file_path, 'wb') as saved_file:
pickler = pickle.Pickler(saved_file)
pickler.dump(calib_mat)
print(calib_mat.round(3))
# remove trackers that are not tracked any more
for t in trackers_list:
if t not in current_loop_trackers_list:
print("\n\tTracker lost : {}".format(t))
trackers_list.remove(t)
# send osc content to all listeners if needed
for l in listeners:
if args.bundles:
bundle_builder = osc_bundle_builder.OscBundleBuilder(
osc_bundle_builder.IMMEDIATELY)
for c in osc_content:
msg = osc_message_builder.OscMessageBuilder(
address=args.oscpattern)
for oscarg in c:
msg.add_arg(oscarg)
bundle_builder.add_content(msg.build())
l.send(bundle_builder.build())
else:
for c in osc_content:
l.send_message(args.oscpattern, c)
#calulate fps
fps = 0
if len(t0_list) > 1:
fps = len(t0_list) / (t0_list[-1] - t0_list[0])
# update state display
if (not set_origin):
print(
"\rtime : {0:8.1f}, fps : {1:4.1f}, nb trackers : {2} ".
format(loop_t0 - program_t0, fps, len(osc_content)),
end="")
# ensure fps is respected to avoid OSC messages queue overflow
while time.perf_counter() - loop_t0 <= 1.0 / args.framerate:
pass
# euclidean_matches = euclidean_sift_vec_match(np.arange(0,len(kp1)), orginal_points, keypoints_prime, des1, des2)
# ransac_loop_v2(img1, img2, kp1, kp2, des1, des2)
# ransac_loop_v3(img1, img2, kp1, kp2, euclidean_matches)
bf_matches = mathching_skimage(img1, kp1, des1, img2, kp2, des2, True)
kps1_3d = get_3d_kps(input_voxels[0], kps[0])
kps2_3d = get_3d_kps(input_voxels[1], kps[1])
m_kps1_3d = []
m_kps2_3d = []
for m in bf_matches:
m_kps1_3d.append(kps1_3d[m[0]])
m_kps2_3d.append(kps2_3d[m[1]])
retval, out, inliers = cv2.estimateAffine3D(np.array(m_kps2_3d),
np.array(m_kps1_3d))
print(out)
new_3d_pts = [input_voxels[0]]
new_3d_pts.append(get_transformed_points(input_voxels[1], out))
pcds = []
for i in range(len(inputs)):
voxel_to_csv(input_voxels[i], './cars2/depth/car_{}.csv'.format(i))
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(input_voxels[i])
pcds.append(pcd)
o3d.io.write_point_cloud("./cars2/depth/car_{}.pcd".format(i), pcd)
# o3d.visualization.draw_geometries([pcds[0]])
cam_points = get_camera_points('/home/sam/Desktop/cam.xml')
log_data = read_log('/home/sam/Downloads/jun29_plane_beaufighter/logs/20100101_080859_UTC_0_beaufighter_revisit_IVER3-3013_Corr.log')
proj = Proj(init='epsg:32633')
proj_longs, proj_lats = proj(log_data['longitude'], log_data['latitude'])
log_points = np.stack([proj_longs, proj_lats, -log_data['dfs_depth']], axis=1)
target_longlat = np.array(proj(14.5031, 35.9243))
longlats = np.stack([proj_longs, proj_lats], axis=1)
dist_from_target = np.linalg.norm(longlats - target_longlat, axis=1)
mask = (log_data['dfs_depth'] > 20) & (dist_from_target < 15)
log_points = log_points[mask]
cam_points = np.random.rand(10, 3)
log_points = np.random.rand(11, 3)
print(cam_points.shape, len(cam_points), cam_points.dtype)
print(log_points.shape, len(log_points), log_points.dtype)
retval, out, inliers = cv2.estimateAffine3D(cam_points, log_points)
print(retval, out, inliers)
# fig = plt.figure()
# ax = fig.gca(projection='3d')
# plt.axis('equal')
# plt.ticklabel_format(useOffset=False)
# # ax.plot(cam_points[:,0], cam_points[:,1], cam_points[:,2], 'bo', markersize=0.5)
# ax.plot(log_points[:,0], log_points[:,1], log_points[:,2], 'go', markersize=0.5)
# ax.plot(result[:,0], result[:,1], result[:,2], 'ro', markersize=0.5)
# plt.show()
def pose_from_point_cloud(img1, img2):
left, right = s3d.load_images(img1)
left2, right2 = s3d.load_images(img2)
matched_features_1, matched_features_2 = s3d.find_n_best_feature_points(
left, left2, 300, 40, False)
features_left_1 = np.array([(f.pt[0], f.pt[1])
for f in matched_features_1])
features_left_2 = np.array([(f.pt[0], f.pt[1])
for f in matched_features_2])
#print(features_left_1, features_left_2)
disparity_1 = s3d.compute_disparity(left, right, False)
disparity_2 = s3d.compute_disparity(left2, right2, False)
points_3d_1, points_3d_2 = matched_points_from_point_cloud(
disparity_1, features_left_1, disparity_2, features_left_2
) # MAKE ONE THAT DOES BOTH IMAGES AT SAME TIME, SINCE LOTS OF THE POINTS DO NOT GET DONE DUES TO DISPARITY FOR SOME REASON, SO THE LEFTOVER ONES ARENT MATCHED
#print(np.transpose(points_3d_1))
#formatted_points_1 = formatted_points_2 = []
#for p in points_3d_1:
_, transformation, _ = cv2.estimateAffine3D(points_3d_1,
points_3d_2,
ransacThreshold=0.97)
#RESHAPE POINT ARRAYS
#print(points_3d_1, points_3d_2)
#affine3d = trans.affine_matrix_from_points(np.transpose(points_3d_1), np.transpose(points_3d_2), False, False)
#print(transformation, affine3d)
transformation = np.append(transformation, [[0.0, 0.0, 0.0, 1.0]], axis=0)
#print(transformation, affine3d)
scale, shear, angles, translation, perspective = trans.decompose_matrix(
transformation)
r = angles
t = translation
"""
h_mat, _ = cv2.findHomography(points_3d_1, points_3d_2, method=cv2.RANSAC)
_, r, t, _ = cv2.decomposeHomographyMat(h_mat, camera_matrix)
print("r, t:", r, t)
global prev_r, prev_t
if len(prev_r) > 0 and len(prev_t) > 0:
best_r = r[0]
for i in r[1:]:
#print(i)
if abs(prev_r[0][0] - i[0][0]) < abs(prev_r[0][0] - best_r[0][0]):
best_r = i
best_t = t[0]
for i in t[1:]:
if abs(prev_t[0][0] - i[0][0]) < abs(prev_t[0][0] - best_t[0][0]):
best_t = i
else:
best_r = r[0]
best_t = t[0]
for i in r:
if i[0][0] > 0.99:
best_r = i
for i in t:
if i[2] > 0:
best_t = i
prev_r = best_r
prev_t = best_t"""
return r, t
return x, y, z
# Transform gps coord to ecef coord
ecef_matrix = gps_matrix.copy()
for zz in range(0, gps_matrix.shape[0]):
x, y, z = gps_to_ecef_pyproj(gps_matrix[zz][0], gps_matrix[zz][1],
gps_matrix[zz][2] - antenna_height)
ecef_matrix[zz][0] = x
ecef_matrix[zz][1] = y
ecef_matrix[zz][2] = z
print("ecef_matrix: ", ecef_matrix)
#Solve 3d affine transformation from chessboard coord to ECEF coord using estimateAffine3D
retval, M, inliers = cv2.estimateAffine3D(world_coord_matrix, ecef_matrix)
if retval:
print("affine transform from CB to ECEF: ", M)
dst, Jacobian = cv2.Rodrigues(M[:, 0:3])
print("rotation: ", np.transpose(dst))
print("inliers: ", cv2.transpose(inliers))
# Calculate the Direction Cosine Matrix
lat = math.radians(gps_matrix[0][0])
lon = math.radians(gps_matrix[0][1])
R_From_NED_TO_ECEF = np.zeros((3, 3), np.float32)
R_From_NED_TO_ECEF[0, 0] = -1 * math.sin(lat) * math.cos(lon)
R_From_NED_TO_ECEF[1, 0] = -1 * math.sin(lat) * math.sin(lon)
R_From_NED_TO_ECEF[2, 0] = math.cos(lat)
R_From_NED_TO_ECEF[0, 1] = -1 * math.sin(lon)
for wi in w:
s = np.insert(s, len(s), float(wi))
s = np.insert(s, len(s), 1)
target = np.vstack((target, np.atleast_2d(s)))
with open(sv) as gi:
data = gi.readlines()
for d in data:
w = d.split()
s = np.array([], np.float32)
for wi in w:
s = np.insert(s, len(s), float(wi))
source = np.vstack((source, np.atleast_2d(s)))
target = target[1 : target.shape[0], :]
source = source[1 : source.shape[0], :]
retval, M, inliers = cv2.estimateAffine3D(source, target)
for i in range(0, source.shape[0]):
d = source[i, :]
d = np.insert(d, len(d), 1)
# print np.mat(M)
# print np.mat(d)
# print '\n'
m = np.mat(M) * np.mat(d).T
print m.T
print target[i, :]
print "\n"
# print M
matchesMask = None
draw_params = dict(matchColor = (0,255,0), # draw matches in green color
singlePointColor = None,
matchesMask = matchesMask, # draw only inliers
flags = 2)
img3 = cv2.drawMatches(img1,kp1,img2,kp2,good,None,**draw_params)
plt.imshow(img3, 'gray'),plt.show()
img1_depth = cv2.imread('car1-depth.png', cv2.IMREAD_GRAYSCALE)
img2_depth = cv2.imread('car2-depth.png', cv2.IMREAD_GRAYSCALE)
img1_depth = depth_to_voxel(img1_depth).astype(np.float32)
img2_depth = depth_to_voxel(img2_depth).astype(np.float32)
print(len(img1_depth), len(img2_depth))
retval, out, inliers = cv2.estimateAffine3D(img1_depth, img2_depth)
print(out)
# img1 = depth_to_voxel(img1, 50)
# # voxel_to_csv(img1, r"C:\Users\karlc\Documents\ut\_y4\CSC420\project\3d_reconstruction\00003\points1.csv")
#
# img2 = cv2.imread(
# r"C:\Users\karlc\Documents\ut\_y4\CSC420\project\3d_reconstruction\00003\depth\0000050-000001635093.png",
# cv2.IMREAD_GRAYSCALE)
# img2 = depth_to_voxel(img2, 50)
# # voxel_to_csv(img2, r"C:\Users\karlc\Documents\ut\_y4\CSC420\project\3d_reconstruction\00003\points2.csv")
#
# img3 = cv2.imread(
# r"C:\Users\karlc\Documents\ut\_y4\CSC420\project\3d_reconstruction\00003\depth\0000090-000002969863.png",
# cv2.IMREAD_GRAYSCALE)
# img3 = depth_to_voxel(img3, 50)
#testing
if __name__ == "__main__":
# DEFINE THESE
# reference_pts are the checkerboard corners from Primary Stereocamera
# other_pts are the checkerboard corners from Secondary Stereocamera
# define theta_guess too
reference_pts = np.load('pts3D.npy')
other_pts = np.load('pts3D.npy')
rot_axis = "Ry" #for guess
# calculate the points to use in RANSAC by rotating the input points from Secondary camera using the guessed angle between the cameras
guessed_pts = R_matrices[rot_axis].dot(other_pts.T).T
homog_pts = cv2.convertPointsToHomogeneous(guessed_pts).reshape(-1, 4)
#use affine RANSAC to get correct points
retval, out_mat, inliers = cv2.estimateAffine3D(guessed_pts, reference_pts)
np.save("guessed_mat.npy", R_matrices[rot_axis])
np.save("affine_mat.npy", out_mat)
affine_pts = out_mat.dot(homog_pts.T).T
#affine_pts = cv2.convertPointsFromHomogeneous(affine_pts).reshape(-1,3)
# plot original points in blue and rotated poitns in red, fixed points in yellow
# (WE WANT RED==YELLOW FOR PERFECT FIT)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
pts_list = [reference_pts, other_pts, guessed_pts, affine_pts]
i = 0
for pts in pts_list:
def cv_transform(pts1, pts2):
ransac_threshold = 1
confidence = 0.99
res, trans, err = cv2.estimateAffine3D(pts1, pts2, ransacThreshold=ransac_threshold, confidence=confidence)
return trans
s = np.array([], np.float32)
for wi in w:
s = np.insert(s, len(s), float(wi))
s = np.insert(s, len(s), 1)
target = np.vstack((target, np.atleast_2d(s)))
with open(sv) as gi:
data = gi.readlines()
for d in data:
w = d.split()
s = np.array([], np.float32)
for wi in w:
s = np.insert(s, len(s), float(wi))
source = np.vstack((source, np.atleast_2d(s)))
target = target[1:target.shape[0], :]
source = source[1:source.shape[0], :]
retval, M, inliers = cv2.estimateAffine3D(source, target)
for i in range(0, source.shape[0]):
d = source[i, :]
d = np.insert(d, len(d), 1)
# print np.mat(M)
# print np.mat(d)
# print '\n'
m = np.mat(M) * np.mat(d).T
print m.T
print target[i, :]
print '\n'
#print M
Maior[1, 0] = 1 Maior[1, 1] = 0 Maior[1, 2] = 0 Maior[2, 0] = 0 Maior[2, 1] = 1 Maior[2, 2] = 0 Maior[3, 0] = 0 Maior[3, 1] = 0 Maior[3, 2] = 1 xy[0, 0] = 1 xy[0, 1] = 2 xy[0, 2] = 3 xy[1, 0] = 4 xy[1, 1] = 5 xy[1, 2] = 6 xy[2, 0] = 7 xy[2, 1] = 8 xy[2, 2] = 9 xy[3, 0] = 10 xy[3, 1] = 11 xy[3, 2] = 12 retval, out, inliers = cv2.estimateAffine3D(Maior, xy, None, inliers=1)
def add_image(imagefile, depfile, ddd=True, seg_img=False, seg_dep=False):
print('Start matching the image in ', imagefile)
image0 = cv2.imread(imagefile)
image = cv2.resize(image0, (newWidth, newHeight))
dep = np.load(depfile)
images = glob.glob('keys/*.jpg')
Ms = []
l = len(images)
for i in range(max(1, l - 2), l + 1):
img0 = cv2.imread('keys/' + str(i) + '.jpg')
img0 = cv2.resize(img0, (newWidth, newHeight))
pts0, pts, kp1, kp2, matches, matchesMask = sift_points(
img0, image, 0.75)
d0 = np.load('keys/deps/0' + str(i) + '.npy')
#dep, _ = corres_imgs(image,img0,dep,d0)
print(' matching the image with image ' + str(i))
print(' -> the translation matrix is :')
if len(pts) != 0:
pts0 = verttt(pts0, d0[0, :, :, 0], 1)
pts = verttt(pts, dep[0, :, :, 0], 1)
retval, M, inliers = cv2.estimateAffine3D(pts,
pts0,
ransacThreshold=12)
#M = rigid_transform_3D(pts.reshape(-1,3), pts0.reshape(-1,3))
else:
M = None
if M is not None:
print(' -> ', M[0])
print(' -> ', M[1])
print(' -> ', M[2])
M0 = np.load('keys/Ms/' + str(i) + '.npy')
Ms.append(mut_trans(M, M0))
else:
print(' -> None')
if l == 0:
Ms.append(np.eye(3, 4))
#M = average_matrix(Ms)
if seg_img:
image = mask_img_rgb(image, l + 1)
if seg_dep:
dep = np.load(depfile[0:10] + depfile[11:])
if len(Ms) != 0:
M = Ms[len(Ms) - 1]
if ddd:
res = vert(image, del_all(image, dep[0, :, :, 0]), M, False, False)
else:
res = vert(image, dep[0, :, :, 0], M, False, False)
if seg_dep:
res = refine_seg_3D(res)
write_ply('keys/res/' + str(l + 1) + '.ply', res)
np.save('keys/Ms/' + str(l + 1) + '.npy', M)
cv2.imwrite('keys/' + str(l + 1) + '.jpg', image0)
print(' the process of image ' + str(l + 1) + ' is finished.')
else:
print(' cannot match image ' + str(l + 1) + '.')
print(' ')
return 0
# Need to find more appropriate way to downsample point cloud data / Open3D way
# doesn't work
# Works with translation too
if __name__ == "__main__":
source = read_point_cloud("../data/heart_scan_processed.txt", format="xyz")
source.transform([[1,0,0,0],[0,-1,0,0],[0,0,-1,0],[0,0,0,1]])
test = read_point_cloud("../data/heart_scan_scaled.txt", format="xyz")
test.transform([[1,0,0,0],[0,-1,0,0],[0,0,-1,0],[0,0,0,1]])
test.transform([[0,-1,0,100],[1,0,0,100],[0,0,1,100],[0,0,0,1]])
# voxel_size = 0.005
# source, target, source_down, target_down, source_fpfh, target_fpfh =
# prepare_dataset(voxel_size, source, test)
draw_registration_result(source, target, np.identity(4))
draw_geometries([source])
draw_geometries([target])
source_arr = np.asmatrix(source.points)
target_arr = np.asmatrix(target.points)
if (len(target_arr) > len(source_arr)):
target_arr = target_arr[:len(source_arr),:]
retval, out, inliers = cv2.estimateAffine3D(source_arr, target_arr,
confidence=0.96)
out = np.vstack([out, [0.0, 0.0, 0.0, 1.0]])
print(out)
draw_registration_result(source, target, out)
pnts3d1, placeHolder, placeHolder = get3DPoints(kp1, kp2, des1, des2) pnts3d2, placeHolder, placeHolder = get3DPoints(kp1, kp3, des1, des3) p1 = pnts3d1[:3].T p2 = pnts3d2[:3].T plotReconstruction(p1) plotReconstruction(p2) if len(p1) > len(p2): minLength = len(p2) else: minLength = len(p1) #print p2 retval, out, inliers = cv2.estimateAffine3D(p2[:minLength], p1[:minLength]) print out print inliers out = np.vstack((out, np.array([0,0,0,1]))) p2 = p2.T p2 = np.vstack((p2, np.ones(len(p2[0])))) p2 = np.dot(p2.T, out) #pnts3d2 = np.dot(A, pnts3d2) #p = np.append( p1.T, p2.T, axis = 0 ) '''pointsForImage3 = np.array([[i.pt[0], i.pt[1]] for i in kp3])
def task2():
# read data
with open('calibration_data.pp', 'rb') as f:
data = pickle.load(f, encoding='latin1')
# find camera exterinsic parameters w.e.t object coordinates
cameraMatrix = data['leftCameraData']['cameraMatrix']
objectPoints = [data['objectPoints'].astype(np.float32)] * 10
left_rvecs, left_tvecs = get_extrinisic_parameters(
data['objectPoints'], data['leftCameraData']['imagePoints'],
data['leftCameraData']['cameraMatrix'])
right_rvecs, right_tvecs = get_extrinisic_parameters(
data['objectPoints'], data['rightCameraData']['imagePoints'],
data['rightCameraData']['cameraMatrix'])
# find object coordinates in camera system
tot_error = 0
X_left = np.zeros((len(objectPoints), 4, objectPoints[0].shape[0]),
dtype=np.float32)
X_right = np.zeros((len(objectPoints), 4, objectPoints[0].shape[0]),
dtype=np.float32)
left_imagePoints = [
d.astype(np.float32).reshape((-1, 1, 2))
for d in data['leftCameraData']['imagePoints']
]
right_imagePoints = [
d.astype(np.float32).reshape((-1, 1, 2))
for d in data['rightCameraData']['imagePoints']
]
for i in range(len(objectPoints)):
R, _ = cv2.Rodrigues(left_rvecs[i])
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = left_tvecs[i][:, 0]
X = np.ones((4, left_imagePoints[i].shape[0]))
X[:3, :] = objectPoints[0].T
X_left[i, :, :] = np.matmul(T, X)
R, _ = cv2.Rodrigues(right_rvecs[i])
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = right_tvecs[i][:, 0]
X = np.ones((4, right_imagePoints[i].shape[0]))
X[:3, :] = objectPoints[0].T
X_right[i, :, :] = np.matmul(T, X)
# Xt = np.matmul(T,X)
# Xt_proj = Xt/Xt[2]
# U = np.matmul(data['rightCameraData']['cameraMatrix'], Xt_proj)
# imgpoints2 = U[:2,:].T.reshape((-1,1,2)).astype(np.float32)
# # imgpoints2, _ = cv2.projectPoints(objectPoints[i], left_rvecs[i], left_tvecs[i], left_cameraMatrix, (0,0,0,0,0))
# error = cv2.norm(left_imagePoints[i], imgpoints2, cv2.NORM_L2)/len(imgpoints2)
# plt.subplot(2,5, i +1)
# plt.plot(left_imagePoints[i][:,0,0],left_imagePoints[i][:,0,1], 'b*'),
# plt.plot(imgpoints2[:,0,0],imgpoints2[:,0,1], 'r+'),
# tot_error += error
print("total error: ", tot_error / len(objectPoints))
# plt.show()
# estiamte Transform matrix using least mean square
N = X_right.shape[0] * X_right.shape[2]
# x0 = np.zeros(6).reshape(-1)
x0 = np.eye(4).reshape(-1)
lower_bound = np.zeros(16)
lower_bound[12:15] = -np.finfo(float).eps
lower_bound[[0, 1, 2, 4, 5, 6, 8, 9, 10]] = -1 - np.finfo(float).eps
lower_bound[[3, 7, 11]] = -np.inf
lower_bound[15] = 1 - np.finfo(float).eps
upper_bound = np.zeros(16)
upper_bound[12:15] = np.finfo(float).eps
upper_bound[[0, 1, 2, 4, 5, 6, 8, 9, 10]] = 1 + np.finfo(float).eps
upper_bound[[3, 7, 11]] = np.inf
upper_bound[15] = 1 + np.finfo(float).eps
bounds = (lower_bound, upper_bound)
diff = X_right - X_left
cost = np.linalg.norm(diff[:, :3, :]) / np.sqrt(N)
print("before optimize", cost)
T_params = least_squares(fun=transform_cost, x0=x0, args=(X_right, X_left))
y_pred = transform(T_params.x, X_right)
diff = y_pred - X_left
cost = np.linalg.norm(diff[:, :3, :]) / np.sqrt(N)
print("after optimize", cost)
# T_params = least_squares(fun=transform_cost,x0=T_params.x,args=(X_right, X_left))
# y_pred = transform(T_params.x, X_right)
# diff = y_pred - X_left
# cost = np.linalg.norm(diff)
# print("after optimize", cost)
src = np.swapaxes(X_right[:, :3, :], 1, 2).reshape(-1, 3)
dst = np.swapaxes(X_left[:, :3, :], 1, 2).reshape(-1, 3)
retval, out, inliers = cv2.estimateAffine3D(src, dst)
rvec, _ = cv2.Rodrigues(out[:3, :3])
# x0 = np.zeros(6)
# x0[:3] = rvec[:,0]
# x0[3:6] = out[:,3]
x0 = np.eye(4)
x0[:3, :] = out
x0 = x0.reshape(-1)
y_pred = transform(x0, X_right)
diff = y_pred - X_left
cost = np.linalg.norm(diff[:, :3, :]) / np.sqrt(N)
print("after cv2 estimation", cost)
rvec = np.array([x0[0], x0[1], x0[2]]).reshape((3, 1))
R, _ = cv2.Rodrigues(rvec)
T = np.array([x0[3], x0[4], x0[5]], dtype=np.float32).reshape((3, 1))
T = np.concatenate((R, T), axis=1).astype(np.float32)
T = np.vstack((T, np.zeros((1, T.shape[1]), dtype=np.float32)))
T[3, 3] = 1
# print(out)
# print(T)
T_params = least_squares(fun=transform_cost, x0=x0, args=(X_right, X_left))
y_pred = transform(T_params.x, X_right)
diff = y_pred - X_left
cost = np.linalg.norm(diff[:, :3, :]) / np.sqrt(N)
print("final optimize", cost)
print(markers)
# markers = np.array([
# [176,186,34],
# [200,685,43],
# [577,684,34],
# [485,241,43]
# ])
markers_zyx = markers[:, ::-1]
print(8.9 * edge_lengths(markers))
im_shape = np.array(active_image.shape)
vertices = np.array([(0, 0, 1), (im_shape[0], 0, 1),
(im_shape[0], im_shape[1], 1), (0, im_shape[1], 1)])
_, transform, _ = cv2.estimateAffine3D(vertices, markers, False)
transform = np.vstack((transform, [0, 0, 0, 1])).transpose()
vertices_aug = np.hstack((vertices, np.array([[1], [1], [1], [1]])))
rotated = np.dot(vertices_aug, transform)
CustomIm = scene.visuals.create_visual_node(CustomImageVisual)
image_visual = CustomIm(active_image,
markers,
parent=view.scene,
cmap='grays')
image_visual.clim = [active_image.min(), active_image.max()]
volume_visual = scene.visuals.Volume(volume.swapaxes(0, 2),
parent=view.scene,
threshold=0.225,
emulate_texture=False)
def createTransformFunc(ptPairs, direction):
"""
Returns a function that takes a point (x,y,z) of a camera coordinate, and returns the robot (x,y,z) for that point
Direction is either "toRob" or "toCam".
If "toCam" it will return a Robot-->Camera transform
If "toRob" it will return a Camera-->Robot transform
"""
ptPairArray = np.asarray(ptPairs)
if direction == "toRob":
pts1 = ptPairArray[:, 0]
pts2 = ptPairArray[:, 1]
if direction == "toCam":
pts1 = ptPairArray[:, 1]
pts2 = ptPairArray[:, 0]
# Generate the transformation matrix
ret, M, mask = cv2.estimateAffine3D(np.float32(pts1),
np.float32(pts2),
confidence = .9999999)
if not ret: printf("ERROR: Transform failed!")
# Returns a function that will perform the transformation between pointset 1 and pointset 2 (if direction is == 1)
transformFunc = lambda x: np.array((M * np.vstack((np.matrix(x).reshape(3, 1), 1)))[0:3, :].reshape(1, 3))[0]
# # TOCAM test
# pts1 = ptPairArray[:, 1]
# pts2 = ptPairArray[:, 0]
# ret, M, mask = cv2.estimateAffine3D(np.float32(pts1),
# np.float32(pts2),
# confidence = .9999999)
# normalFunc = lambda x: np.array((M * np.vstack((np.matrix(x).reshape(3, 1), 1)))[0:3, :].reshape(1, 3))[0]
# print("STARTING\n Normal M, toCam")
# m2 = np.vstack((M, [0, 0, 0, 1]))
# print(m2)
# print("Normal toCam input 15, 30, 45", normalFunc((15, 30, 45)))
# print("Inverse (toRob)\n", np.linalg.inv(m2))
#
#
#
# # TOROB test
# pts1 = ptPairArray[:, 0]
# pts2 = ptPairArray[:, 1]
# ret, M, mask = cv2.estimateAffine3D(np.float32(pts1),
# np.float32(pts2),
# confidence = .9999999)
#
# print("\nNormal M, toRob")
# m = np.vstack((M, [0, 0, 0, 1]))
# print(m)
#
# # print("Built inverse: ")
# def inverse(m):
# i = np.linalg.inv(m)
# # 0 1 2 3
# m = [[ i[0][0], m[][], m[][], -1/i[3][0]], # 0
# [ m[][], m[1][1], m[][], m[3][1]], # 1
# [ m[][], i[][], m[2][2], m[3][2]], # 2
# [ 0, 0, 0, 1]] # 3
#
# print("Inverse (toCam)\n", np.linalg.inv(m))
# invFunc = lambda x: np.array((np.linalg.inv(m) * np.vstack((np.matrix(x).reshape(3, 1), 1)))[0:3, :].reshape(1, 3))[0]
# print("Inverse (toCam) input 15, 30, 45", invFunc((15, 30, 45)))
"""
Breakdown of function. Here's an example of transforming [95, -35, 530] cam which is [5, 15, 15] in the robot grid
First:
[95, -35, 530]
np.vstack((np.matrix(input).reshape(3, 1), 1)) is applied, converts it to a vertical array
[[ 95]
[-35]
[530]
[ 1]]
M * np.vstack((np.matrix(x).reshape(3, 1), 1)) (transformation matrix multiplied by the previous number)
[[ 5.8371337 ]
[ 15.72722685]
[ 14.5007519 ]]
There you go. The rest of it is simply converting that matrix into
[5.83, 15.72, 14.5]
but you know, without the rounding.
"""
return transformFunc
