def runCPDRegistration(self, sourceLM, sourceSLM, targetSLM, parameters):
from open3d import geometry
from open3d import utility
sourceArrayCombined = np.append(sourceSLM, sourceLM, axis=0)
targetArray = np.asarray(targetSLM)
#Convert to pointcloud for scaling
sourceCloud = geometry.PointCloud()
sourceCloud.points = utility.Vector3dVector(sourceArrayCombined)
targetCloud = geometry.PointCloud()
targetCloud.points = utility.Vector3dVector(targetArray)
cloudSize = np.max(targetCloud.get_max_bound() -
targetCloud.get_min_bound())
targetCloud.scale(25 / cloudSize, center=False)
sourceCloud.scale(25 / cloudSize, center=False)
#Convert back to numpy for cpd
sourceArrayCombined = np.asarray(sourceCloud.points, dtype=np.float32)
targetArray = np.asarray(targetCloud.points, dtype=np.float32)
registrationOutput = self.cpd_registration(targetArray,
sourceArrayCombined,
parameters["CPDIterations"],
parameters["CPDTolerence"],
parameters["alpha"],
parameters["beta"])
deformed_array, _ = registrationOutput.register()
#Capture output landmarks from source pointcloud
fiducial_prediction = deformed_array[-len(sourceLM):]
fiducialCloud = geometry.PointCloud()
fiducialCloud.points = utility.Vector3dVector(fiducial_prediction)
fiducialCloud.scale(cloudSize / 25, center=False)
return np.asarray(fiducialCloud.points)
def visualize_pc(pc, mask=None):
'''
pc - 3 x n
mask - 1 x n, [0,1]
'''
pc = pc.permute(1, 0) # n x 3
pc = pc.cpu().detach().numpy()
pcd = geometry.PointCloud()
pcd.points = utility.Vector3dVector(pc)
if mask is not None:
mask = mask / mask.max()
#mask=mask.pow(0.5) #dilate the values
mask = mask.expand(3, -1).permute(1, 0) #nx3
red = mask.new(mask.shape).zero_()
red[:, 0] += 1.0
green = mask.new(mask.shape).zero_()
green[:, 1] += 1.0
blue = mask.new(mask.shape).zero_()
blue[:, 2] += 1.0
less = ((2 * mask).mul(green) + (1.0 - (2 * mask)).mul(blue)).mul(
mask.lt(0.5).float())
more = (((mask - 0.5) * 2).mul(red) +
(1.0 - ((mask - 0.5) * 2)).mul(green)).mul(
mask.ge(0.5).float())
mask = less + more
mask_max, _ = mask.max(dim=1, keepdim=True)
mask = mask.div(mask_max)
mask = mask.cpu().detach().numpy()
pcd.colors = utility.Vector3dVector(mask)
visualization.draw_geometries([pcd])
def __find_plane(self, x, y, z, Trans, Rot, mean_pt, debug=False):
#mean_pt = mean_pt/np.linalg.norm(mean_pt)
pts = np.array([x, y, z]).T
print(pts.shape)
t_size = np.min((500, pts.shape[0]))
t_pts = np.random.choice(np.arange(pts.shape[0]),
size=t_size,
replace=False)
pts = pts[t_pts]
print(pts.shape)
all_points = open3d.utility.Vector3dVector(pts)
pc = geometry.PointCloud(all_points)
plane, success_pts = pc.segment_plane(0.05, int(pts.shape[0] * 0.7),
500)
plane = np.array(plane)
# print("Plane:", pts.shape, len(success_pts))
if debug:
print("Plane: ", plane)
print("Mean pt: ", mean_pt)
plane_debug = plane.copy()
d = np.dot(mean_pt, plane[:3])
if d > 0:
plane = -plane
plane_ = plane[:3] #np.matmul(R.from_quat(Rot).as_dcm(), plane[:3].T)
plane_[1] = 0
plane_ /= np.linalg.norm(plane_)
y = np.array([0, -1, 0])
z = np.cross(plane_, y)
rot = np.array([plane_, y, z]).T
# print(rot, np.linalg.det(rot), R.from_dcm(rot).as_euler('ZYX', degrees=True))
orientation = R.from_dcm(rot).as_quat()
if debug:
self.debugPlanePlotter(pts, plane, orientation, position=mean_pt)
normal_vec = Pose()
normal_vec.position.x = plane_[0]
normal_vec.position.y = plane_[1]
# normal_vec.position.z = plane[2]
#orientation = [0, 0, 0, 1]#self.__find_rot(plane[0:3])
#normal_vec.orientation.x = orientation[0]
#normal_vec.orientation.y= orientation[1]
#normal_vec.orientation.z= orientation[2]
#normal_vec.orientation.w = orientation[3]
vecs = PoseArray()
vecs.poses.append(normal_vec)
header = std_msgs.msg.Header()
header.stamp = self.stamp_now #rospy.Time.now()
header.frame_id = "orange"
vecs.header = header
self.__normal_vec_publisher.publish(vecs)
# print(plane)
return orientation
def __init__(self, model_orig_pcd, sample_pcd):
self.model_pcd_original = model_orig_pcd
self.XYZ_model = np.asarray(self.model_pcd_original.points)
self.XYZ_min_model = np.min(self.XYZ_model, axis=0)
self.XYZ_max_model = np.max(self.XYZ_model, axis=0)
self.diameter_model = np.sqrt(
np.square(self.XYZ_max_model[0] - self.XYZ_min_model[0]) +
np.square(self.XYZ_max_model[1] - self.XYZ_min_model[1]) +
np.square(self.XYZ_max_model[2] - self.XYZ_min_model[2]))
self.model_pcd = geometry.PointCloud(
) # To keep resized model points that can match the size of a sample
# point cloud.
self.sample_pcd = sample_pcd
def loadAndScaleFiducials(self, fiducialPath, scaling):
from open3d import geometry
from open3d import utility
sourceLandmarkNode = slicer.util.loadMarkups(fiducialPath)
self.RAS2LPSTransform(sourceLandmarkNode)
point = [0, 0, 0]
sourceLandmarks_np = np.zeros(
shape=(sourceLandmarkNode.GetNumberOfFiducials(), 3))
for i in range(sourceLandmarkNode.GetNumberOfFiducials()):
sourceLandmarkNode.GetMarkupPoint(0, i, point)
sourceLandmarks_np[i, :] = point
slicer.mrmlScene.RemoveNode(sourceLandmarkNode)
cloud = geometry.PointCloud()
cloud.points = utility.Vector3dVector(sourceLandmarks_np)
cloud.scale(scaling, center=False)
fiducialVTK = self.convertPointsToVTK(cloud.points)
return fiducialVTK
def _draw_points(points,
vis,
points_size=2,
point_color=(0.5, 0.5, 0.5),
mode='xyz'):
"""Draw points on visualizer.
Args:
points (numpy.array | torch.tensor, shape=[N, 3+C]):
points to visualize.
vis (:obj:`open3d.visualization.Visualizer`): open3d visualizer.
points_size (int): the size of points to show on visualizer.
Default: 2.
point_color (tuple[float]): the color of points.
Default: (0.5, 0.5, 0.5).
mode (str): indicate type of the input points, avaliable mode
['xyz', 'xyzrgb']. Default: 'xyz'.
Returns:
tuple: points, color of each point.
"""
vis.get_render_option().point_size = points_size # set points size
if isinstance(points, torch.Tensor):
points = points.cpu().numpy()
points = points.copy()
pcd = geometry.PointCloud()
if mode == 'xyz':
pcd.points = o3d.utility.Vector3dVector(points[:, :3])
points_colors = np.tile(np.array(point_color), (points.shape[0], 1))
elif mode == 'xyzrgb':
pcd.points = o3d.utility.Vector3dVector(points[:, :3])
points_colors = points[:, 3:6]
# normalize to [0, 1] for open3d drawing
if not ((points_colors >= 0.0) & (points_colors <= 1.0)).all():
points_colors /= 255.0
else:
raise NotImplementedError
pcd.colors = o3d.utility.Vector3dVector(points_colors)
vis.add_geometry(pcd)
return pcd, points_colors
def test_GFP():
from open3d import visualization, geometry, utility
test_points, test_labels = load_h5(test_h5_path)
nr_points = ModellingParameters.NUM_POINTS_UPSAMPLE
model = GFPNet(nr_points)
model.compile(optimizer='adam',
loss='mean_squared_error',
metrics=['mse', 'accuracy'])
# print the model summary
model.load_weights(net_params.PATH_TO_WEIGHTS)
print(model.summary())
primitive_path = PATHS.PATH_TO_PRIMITIVES.CAR.root + "CarPrimitive_15_500.off"
io_primitive = IoPrimitive(primitive_path)
io_primitive.load_primitive(normalize=False)
pathlist = Path(PATHS.NETWORK.root).glob('*.{0}'.format('off'))
for path in pathlist:
lidar_cloud_path = str(path)
file_name = lidar_cloud_path.split("\\")[-1]
label_path = PATHS.NETWORK.TEST_MODELLED + file_name
cloud_path = PATHS.NETWORK.TEST_CLOUD + file_name
observation_points = read_off_file(cloud_path)
io_observation_cloud = IoObservation(observation_points)
io_primitive.scale_relative_to(io_observation_cloud)
io_primitive.align_primitive_on_z_axis(io_observation_cloud)
io_primitive.compute_normals()
modelled_primitive_points = read_off_file(label_path)
io_modelled_primitive = IoObservation(modelled_primitive_points)
eval_X, boolIndexes = make_samples(io_observation_cloud,
io_primitive,
io_modelled_primitive,
usePrimitivePoints=False,
generate_for='test')
eval_X = upsample_point_set(
sample_collection=eval_X,
points_count=ModellingParameters.NUM_POINTS_UPSAMPLE)
cloud_bef = geometry.PointCloud()
cloud_bef.points = utility.Vector3dVector(
np.asarray(io_primitive.point_cloud.points))
cloud_bef.normals = utility.Vector3dVector(
np.asarray(io_primitive.point_cloud.normals))
cloud_bef.paint_uniform_color([255, 0, 0])
cloud_lidar = geometry.PointCloud()
cloud_lidar.points = utility.Vector3dVector(
np.asarray(io_observation_cloud.point_cloud))
cloud_lidar.paint_uniform_color([0, 0, 0])
cloud_modelled = geometry.PointCloud()
cloud_modelled.points = utility.Vector3dVector(
np.asarray(io_modelled_primitive.point_cloud.points))
cloud_modelled.paint_uniform_color([255, 255, 0])
visualization.draw_geometries([cloud_bef, cloud_lidar, cloud_modelled])
final_pts = []
idx = 0
for i in range(0, len(eval_X)):
pred = eval_X[i].reshape(-1, nr_points, 3)
points = model.predict(pred)
final_pts.append(points)
idx = i
final_pts = np.reshape(final_pts, newshape=(len(final_pts), 3))
print('Final pts len : ', len(final_pts))
final_pts = [
point * ModellingParameters.CAR.SCALE for point in final_pts
]
true_indexes = [i for i, val in enumerate(boolIndexes) if val]
for i in true_indexes:
cloud_bef.colors[i] = [0, 255, 0]
import pclpy.pcl.point_types as ptype
aux = ptype.PointXYZ()
new_points = []
for i in range(0, len(final_pts)):
val = io_primitive.point_cloud.points[true_indexes[i]] + (
final_pts[i] - ModellingParameters.NORMALIZATION_CENTER)
aux.x = val[0]
aux.y = val[1]
aux.z = val[2]
new_points.append(val)
# transform_neighbor_points_for(i, aux, srcPrimitive, None)
cloud_aft = geometry.PointCloud()
cloud_aft.points = utility.Vector3dVector(new_points)
cloud_aft.paint_uniform_color([0, 0, 255])
# cloud.normals = utility.Vector3dVector(cloud_points[:,3:6])
visualization.draw_geometries(
[cloud_bef, cloud_aft, cloud_lidar, cloud_modelled])