def ransac_based_on_correspondence(source_keypts, target_keypts, source_desc, target_desc):
source_pc = open3d.PointCloud()
source_pc.points = open3d.utility.Vector3dVector(source_keypts)
target_pc = open3d.PointCloud()
target_pc.points = open3d.utility.Vector3dVector(target_keypts)
voxel_size = 0.05
# source_pc = open3d.geometry.voxel_down_sample(source_pc, voxel_size)
open3d.geometry.estimate_normals(source_pc, open3d.KDTreeSearchParamHybrid(radius=voxel_size * 2, max_nn=30))
# target_pc = open3d.geometry.voxel_down_sample(target_pc, voxel_size)
open3d.geometry.estimate_normals(target_pc, open3d.KDTreeSearchParamHybrid(radius=voxel_size * 2, max_nn=30))
start_time = time.time()
corr = calculate_M(source_desc[0:2000], target_desc[0:2000])
print(time.time() - start_time)
print("Num of correspondence", len(corr))
result = open3d.registration_ransac_based_on_correspondence(
source=source_pc,
target=target_pc,
corres=open3d.utility.Vector2iVector(corr),
max_correspondence_distance=0.05,
estimation_method=open3d.registration.TransformationEstimationPointToPoint(False),
ransac_n=4,
criteria=open3d.registration.RANSACConvergenceCriteria(4000000, 500)
)
return result
def estimateTransform(self, source, target):
source = od.read_point_cloud(source)
target = od.read_point_cloud(target)
current_transformation = self.base_transform
for scale in range(len(self.voxel_radius)):
iterations = self.max_iter[scale]
radius = self.voxel_radius[scale]
source_down = od.voxel_down_sample(source, radius)
target_down = od.voxel_down_sample(target, radius)
od.estimate_normals(
source_down,
od.KDTreeSearchParamHybrid(radius=2 * radius,
max_nn=self.max_nn))
od.estimate_normals(
target_down,
od.KDTreeSearchParamHybrid(radius=2 * radius,
max_nn=self.max_nn))
result_icp = od.registration_colored_icp(
source_down, target_down, radius, current_transformation,
od.ICPConvergenceCriteria(
relative_fitness=self.relative_fitness,
relative_rmse=self.relative_rmse,
max_iteration=iterations))
current_transformation = result_icp.transformation
# self.draw_registration_result_original_color(
# source_down, target_down, current_transformation)
return current_transformation
def sample_point_cloud_feature(point_cloud: op3.PointCloud,
voxel_size: float,
verbose=False):
"""
Down sample and sample the point cloud feature
:param point_cloud: an object of Open3D
:param voxel_size: a float value, that is how sparse you want the sample points is
:param verbose: a boolean value, display notification and visualization when True and no notification when False
:return: 2 objects of Open3D, that are down-sampled point cloud and point cloud feature fpfh
"""
if verbose: print(":: Downsample with a voxel size %.3f." % voxel_size)
point_cloud_down_sample = op3.voxel_down_sample(point_cloud, voxel_size)
radius_normal = voxel_size * 2
if verbose:
print(":: Estimate normal with search radius %.3f." % radius_normal)
op3.estimate_normals(
point_cloud_down_sample,
op3.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30))
radius_feature = voxel_size * 5
if verbose:
print(":: Compute FPFH feature with search radius %.3f." %
radius_feature)
point_cloud_fpfh = op3.compute_fpfh_feature(
point_cloud_down_sample,
op3.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100))
return point_cloud_down_sample, point_cloud_fpfh
def ransac_based_on_feature_matching(source_keypts, target_keypts, source_desc, target_desc):
source_pc = open3d.PointCloud()
source_pc.points = open3d.utility.Vector3dVector(source_keypts)
target_pc = open3d.PointCloud()
target_pc.points = open3d.utility.Vector3dVector(target_keypts)
source_feature = open3d.registration.Feature()
source_feature.data = source_desc.transpose()
target_feature = open3d.registration.Feature()
target_feature.data = target_desc.transpose()
voxel_size = 0.05
# source_pc = open3d.geometry.voxel_down_sample(source_pc, voxel_size)
open3d.geometry.estimate_normals(source_pc, open3d.KDTreeSearchParamHybrid(radius=voxel_size * 2, max_nn=30))
# target_pc = open3d.geometry.voxel_down_sample(target_pc, voxel_size)
open3d.geometry.estimate_normals(target_pc, open3d.KDTreeSearchParamHybrid(radius=voxel_size * 2, max_nn=30))
# source_feature = open3d.registration.compute_fpfh_feature(source_pc, open3d.KDTreeSearchParamHybrid(radius=voxel_size * 5, max_nn=100))
# target_feature = open3d.registration.compute_fpfh_feature(target_pc, open3d.KDTreeSearchParamHybrid(radius=voxel_size * 5, max_nn=100))
result = open3d.registration_ransac_based_on_feature_matching(
source=source_pc,
target=target_pc,
source_feature=source_feature,
target_feature=target_feature,
max_correspondence_distance=0.05,
estimation_method=open3d.registration.TransformationEstimationPointToPoint(False),
ransac_n=3,
checkers=[],
criteria=open3d.registration.RANSACConvergenceCriteria(4000000, 500)
)
return result
def _get_features(cloud):
o3.estimate_normals(
cloud,
o3.KDTreeSearchParamHybrid(radius=Param["normal_radius"],
max_nn=Param["normal_max_nn"]))
return o3.compute_fpfh_feature(
cloud,
o3.KDTreeSearchParamHybrid(radius=Param["feature_radius"],
max_nn=Param["feature_max_nn"]))
def feature_(pcd):
o3d.estimate_normals(
pcd,
o3d.KDTreeSearchParamHybrid(radius=param['radius_normal'],
max_nn=param['maxnn_normal']))
ft = o3d.compute_fpfh_feature(
pcd,
o3d.KDTreeSearchParamHybrid(radius=param['radius_feature'],
max_nn=param['maxnn_feature']))
return ft
def registration(voxel_size, threshold, source_points, target_points,
trans_init):
source, target, source_down, target_down, source_fpfh, target_fpfh = \
prepare_dataset(voxel_size, source_points, target_points, trans_init)
result_ransac = execute_global_registration(source_down, target_down,
source_fpfh, target_fpfh,
voxel_size)
# perform ICP registration
o3d.estimate_normals(source,
search_param=o3d.KDTreeSearchParamHybrid(radius=1,
max_nn=30))
o3d.estimate_normals(target,
search_param=o3d.KDTreeSearchParamHybrid(radius=1,
max_nn=30))
trans_init = result_ransac.transformation
# print("Initial alignment")
evaluation = o3d.registration.evaluate_registration(
source, target, threshold, trans_init)
# print(evaluation)
# print("Apply point-to-point ICP")
reg_p2p = o3d.registration.registration_icp(
source, target, threshold, trans_init,
o3d.registration.TransformationEstimationPointToPoint())
# print(reg_p2p)
# print("Transformation is:")
# print(reg_p2p.transformation)
# draw_registration_result(source, target, reg_p2p.transformation)
# second ICP
trans_init2 = reg_p2p.transformation
# print("Apply 2nd point-to-point ICP")
reg_p2p2 = o3d.registration.registration_icp(
source, target, threshold, trans_init2,
o3d.registration.TransformationEstimationPointToPoint())
# draw_registration_result(source, target, reg_p2p2.transformation)
# third ICP
trans_init3 = reg_p2p2.transformation
# print("Apply 3rd point-to-point ICP")
reg_p2p3 = o3d.registration.registration_icp(
source, target, threshold, trans_init3,
o3d.registration.TransformationEstimationPointToPoint())
# print('checking rms')
# print(reg_p2p3.inlier_rmse)
# print(reg_p2p3)
# print("Transformation 3 is:")
# print(reg_p2p3.transformation)
# print("")
# draw_registration_result(source, target, reg_p2p3.transformation)
return reg_p2p3
def preprocess_point_cloud(pcd, voxel_size):
pn.estimate_normals(pcd)
pcd_down = pn.voxel_down_sample(pcd, voxel_size)
radius_normal = voxel_size * 2
pn.estimate_normals(
pcd_down, pn.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=10))
radius_feature = voxel_size * 5
pcd_fpfh = pn.compute_fpfh_feature(
pcd_down, pn.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=50))
return pcd_down, pcd_fpfh
def preprocess_point_cloud(pcd, voxel_size):
print(":: Downsample with a voxel size %.3f." % voxel_size)
pcd_down = op3.voxel_down_sample(pcd, voxel_size)
radius_normal = voxel_size * 2
print(":: Estimate normal with search radius %.3f." % radius_normal)
op3.estimate_normals(
pcd_down, op3.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30))
radius_feature = voxel_size * 5
print(":: Compute FPFH feature with search radius %.3f." % radius_feature)
pcd_fpfh = op3.compute_fpfh_feature(
pcd_down, op3.KDTreeSearchParamHybrid(radius=radius_feature,
max_nn=100))
return pcd_down, pcd_fpfh
def ViewPLY(path):
# 読み込み
pointcloud = open3d.read_point_cloud(path)
# ダウンサンプリング
# 法線推定できなくなるので不採用
pointcloud = open3d.voxel_down_sample(pointcloud, 10)
# 法線推定
open3d.estimate_normals(pointcloud,
search_param=open3d.KDTreeSearchParamHybrid(
radius=20, max_nn=30))
# 法線の方向を視点ベースでそろえる
open3d.orient_normals_towards_camera_location(pointcloud,
camera_location=np.array(
[0., 10., 10.],
dtype="float64"))
# 可視化
open3d.draw_geometries([pointcloud])
# numpyに変換
points = np.asarray(pointcloud.points)
normals = np.asarray(pointcloud.normals)
print("points:{}".format(points.shape[0]))
X, Y, Z = Disassemble(points)
# OBB生成
_, _, length = buildOBB(points)
print("OBB_length: {}".format(length))
return points, X, Y, Z, normals, length
def o3d_estimate_normals(source):
# downsample and estimate normals
source = o3d.voxel_down_sample(source, voxel_size = 2.5)
o3d.estimate_normals(source, search_param = o3d.KDTreeSearchParamHybrid(
radius = 5, max_nn = 30))
return source
def NormalEstimate(points):
""" 法線推定 """
#点群をnp配列⇒open3d形式に
pointcloud = open3d.PointCloud()
pointcloud.points = open3d.Vector3dVector(points)
# 法線推定
open3d.estimate_normals(pointcloud,
search_param=open3d.KDTreeSearchParamHybrid(
radius=5, max_nn=30))
# 法線の方向を視点ベースでそろえる
open3d.orient_normals_towards_camera_location(pointcloud,
camera_location=np.array(
[0., 10., 10.],
dtype="float64"))
#nキーで法線表示
#open3d.draw_geometries([pointcloud])
#法線をnumpyへ変換
normals = np.asarray(pointcloud.normals)
return normals
def NormalEstimate(points):
#objファイルから点群取得
# points = loadOBJ(path)
print("points:{}".format(points.shape[0]))
#点群をnp配列⇒open3d形式に
pointcloud = open3d.PointCloud()
pointcloud.points = open3d.Vector3dVector(points)
# 法線推定
open3d.estimate_normals(pointcloud,
search_param=open3d.KDTreeSearchParamHybrid(
radius=5, max_nn=30))
# 法線の方向を視点ベースでそろえる
open3d.orient_normals_towards_camera_location(pointcloud,
camera_location=np.array(
[0., 10., 10.],
dtype="float64"))
#nキーで法線表示
#open3d.draw_geometries([pointcloud])
#法線をnumpyへ変換
normals = np.asarray(pointcloud.normals)
#OBB生成
#(最適化の条件にも使いたい)
# _, _, length = buildOBB(points)
# print("OBB_length: {}".format(length))
return normals
def _preprocessPointCloud(self, pcd, voxelSize):
print("Downsample with a voxel size %.3f." % voxelSize)
# pcd_down = pcd.voxel_down_sample(voxelSize)
pcd_down = open3d.voxel_down_sample(pcd, voxelSize)
# cl, pcd_out_remove = open3d.statistical_outlier_removal(pcd_down, nb_neighbors=20, std_ratio=2.0)
radius_normal = voxelSize * 2
print("Estimate normal with search radius %.3f." % radius_normal)
# pcd_down.estimate_normals(open3d.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30))
open3d.estimate_normals(pcd_down, open3d.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30))
radius_feature = voxelSize * 5
print(":: Compute FPFH feature with search radius %.3f." % radius_feature)
pcd_fpfh = open3d.compute_fpfh_feature(
pcd_down,open3d.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100))
return pcd_down, pcd_fpfh
def post_process(affordance_map, class_pred,
color_input, color_bg,
depth_input, depth_bg, camera_intrinsic):
## scale the images to the proper value
color_input = np.asarray(color_input, dtype=np.float32) / 255
color_bg = np.asarray(color_bg, dtype=np.float32) / 255
depth_input = np.asarray(depth_input, dtype=np.float64) / 10000
depth_bg = np.asarray(depth_bg, dtype=np.float64) / 10000
## get foreground mask
frg_mask_color = ~(np.sum(abs(color_input-color_bg) < 0.3, axis=2) == 3)
frg_mask_depth = np.logical_and((abs(depth_input-depth_bg) > 0.02), (depth_bg != 0))
foreground_mask = np.logical_or(frg_mask_color, frg_mask_depth)
## project depth to camera space
pix_x, pix_y = np.meshgrid(np.arange(640), np.arange(480))
cam_x = (pix_x - camera_intrinsic[0][2]) * depth_input/camera_intrinsic[0][0]
cam_y = (pix_y - camera_intrinsic[1][2]) * depth_input/camera_intrinsic[1][1]
cam_z = depth_input
depth_valid = (np.logical_and(foreground_mask, cam_z) != 0)
input_points = np.array([cam_x[depth_valid], cam_y[depth_valid], cam_z[depth_valid]]).transpose()
## get the foreground point cloud
pcd = open3d.PointCloud()
pcd.points = open3d.Vector3dVector(input_points)
open3d.geometry.estimate_normals(pcd,
search_param=open3d.KDTreeSearchParamHybrid(radius = 0.1, max_nn = 50)
)
## flip normals to point towards camera
open3d.geometry.orient_normals_towards_camera_location(pcd, np.array([0.,0.,0.]))
pcd_normals = np.asarray(pcd.normals)
## reproject the normals back to image plane
pix_x = np.round((input_points[:,0] * camera_intrinsic[0][0] / input_points[:,2] + camera_intrinsic[0][2]))
pix_y = np.round((input_points[:,1] * camera_intrinsic[1][1] / input_points[:,2] + camera_intrinsic[1][2]))
surface_normals_map = np.zeros(color_input.shape)
n = 0
for n, (x,y) in enumerate(zip(pix_x, pix_y)):
x,y = int(x), int(y)
surface_normals_map[y,x,0] = pcd_normals[n,0]
surface_normals_map[y,x,1] = pcd_normals[n,1]
surface_normals_map[y,x,2] = pcd_normals[n,2]
## Compute standard deviation of local normals (baseline)
mean_std_norms = np.mean(stdev_filter(surface_normals_map, 25), axis=2)
baseline_score = 1 - mean_std_norms/mean_std_norms.max()
## Set affordance to 0 for regions with high surface normal variance
affordance_map[baseline_score < 0.1] = 0
affordance_map[~foreground_mask] = 0
class_pred[baseline_score < 0.1] = 0
class_pred[~foreground_mask] = 0
affordance_map[~class_pred.astype(np.bool)] = 0
affordance_map = gaussian_filter(affordance_map, 4)
return surface_normals_map, affordance_map, class_pred
def preprocess_point_cloud(pointcloud, voxel_size):
keypoints = open3d.voxel_down_sample(pointcloud, voxel_size)
radius_normal = voxel_size * 2
view_point = np.array([0., 10., 10.], dtype="float64")
open3d.estimate_normals(keypoints,
search_param=open3d.KDTreeSearchParamHybrid(
radius=radius_normal, max_nn=30))
open3d.orient_normals_towards_camera_location(keypoints,
camera_location=view_point)
radius_feature = voxel_size * 5
fpfh = open3d.compute_fpfh_feature(
keypoints,
search_param=open3d.KDTreeSearchParamHybrid(radius=radius_feature,
max_nn=100))
return keypoints, fpfh
def register2Fragments(id1, id2, pcdpath, keyptspath, descpath, desc_name='ppf'):
cloud_bin_s = f'cloud_bin_{id1}'
cloud_bin_t = f'cloud_bin_{id2}'
# 1. load point cloud, keypoints, descriptors
original_source_pc = get_pcd(pcdpath, cloud_bin_s)
original_target_pc = get_pcd(pcdpath, cloud_bin_t)
print("original source:", original_source_pc)
print("original target:", original_target_pc)
# downsample and estimate the normals
voxel_size = 0.02
source_pc = open3d.geometry.voxel_down_sample(original_source_pc, voxel_size)
open3d.geometry.estimate_normals(source_pc, open3d.KDTreeSearchParamHybrid(radius=voxel_size * 2, max_nn=30))
target_pc = open3d.geometry.voxel_down_sample(original_target_pc, voxel_size)
open3d.geometry.estimate_normals(target_pc, open3d.KDTreeSearchParamHybrid(radius=voxel_size * 2, max_nn=30))
print("downsampled source:", source_pc)
print("downsampled target:", target_pc)
# load keypoints and descriptors
source_keypts = get_keypts(keyptspath, cloud_bin_s)
target_keypts = get_keypts(keyptspath, cloud_bin_t)
# print(source_keypts.shape)
source_desc = get_desc(descpath, cloud_bin_s, desc_name=desc_name)
target_desc = get_desc(descpath, cloud_bin_t, desc_name=desc_name)
# 2. ransac
# ransac_result = ransac_based_on_correspondence(source_keypts, target_keypts, source_desc, target_desc)
ransac_result = ransac_based_on_feature_matching(source_keypts, target_keypts, source_desc, target_desc)
print("RANSAC Correspondence_set:", len(ransac_result.correspondence_set))
print(ransac_result.transformation)
# 3. refine with ICP
icp_result = icp_refine(source_pc, target_pc, ransac_result.transformation, voxel_size * 0.4)
print("ICP Correspondence_set:", len(ransac_result.correspondence_set))
print(icp_result.transformation)
# 4. transform the target_pc, so that source and target are in the same coordinates.
target_pc.transform(icp_result.transformation)
# 5. compute the alignment
start_time = time.time()
align = cal_alignment(original_source_pc, original_target_pc, 0.05)
print(time.time() - start_time)
print("align:", align)
print("trans:", icp_result.transformation)
def _estimate_pointcloud_normals_unorganized(points):
assert points.shape[1] == 3
nonnan = ~np.isnan(points).any(axis=1)
points_open3d = open3d.PointCloud()
points_open3d.points = open3d.Vector3dVector(points[nonnan])
open3d.estimate_normals(
points_open3d,
search_param=open3d.KDTreeSearchParamHybrid(radius=0.1, max_nn=30),
)
return np.asarray(points_open3d.normals)
def predict(color_input, color_bg, depth_input, depth_bg, camera_intrinsic):
## scale the images to the proper value
color_input = np.asarray(color_input, dtype=np.float32) / 255
color_bg = np.asarray(color_bg, dtype=np.float32) / 255
depth_input = np.asarray(depth_input, dtype=np.float64) / 10000
depth_bg = np.asarray(depth_bg, dtype=np.float64) / 10000
## get foreground mask
frg_mask_color = ~(np.sum(abs(color_input-color_bg) < 0.3, axis=2) == 3)
frg_mask_depth = np.logical_and((abs(depth_input-depth_bg) > 0.02), (depth_bg != 0))
foreground_mask = np.logical_or(frg_mask_color, frg_mask_depth)
## project depth to camera space
pix_x, pix_y = np.meshgrid(np.arange(640), np.arange(480))
cam_x = (pix_x - camera_intrinsic[0][2]) * depth_input/camera_intrinsic[0][0]
cam_y = (pix_y - camera_intrinsic[1][2]) * depth_input/camera_intrinsic[1][1]
cam_z = depth_input
depth_valid = (np.logical_and(foreground_mask, cam_z) != 0)
input_points = np.array([cam_x[depth_valid], cam_y[depth_valid], cam_z[depth_valid]]).transpose()
## get the foreground point cloud
pcd = open3d.PointCloud()
pcd.points = open3d.Vector3dVector(input_points)
open3d.estimate_normals(pcd, search_param=open3d.KDTreeSearchParamHybrid(radius = 0.1, max_nn = 50))
pcd_normals = np.asarray(pcd.normals)
## flip normals to point towards sensor
center = [0,0,0]
for k in range(input_points.shape[0]):
p1 = center - input_points[k][:]
p2 = pcd_normals[k][:]
x = np.cross(p1,p2)
angle = np.arctan2(np.sqrt((x*x).sum()), p1.dot(p2.transpose()))
if (angle > -np.pi/2 and angle < np.pi/2):
pcd_normals[k][:] = -pcd_normals[k][:]
## reproject the normals back to image plane
pix_x = np.round((input_points[:,0] * camera_intrinsic[0][0] / input_points[:,2] + camera_intrinsic[0][2]))
pix_y = np.round((input_points[:,1] * camera_intrinsic[1][1] / input_points[:,2] + cam_intrinsic[1][2]))
surface_normals_map = np.zeros(color_input.shape)
for n, (x,y) in enumerate(zip(pix_x, pix_y)):
x,y = int(x), int(y)
surface_normals_map[y,x,0] = pcd_normals[n,0]
surface_normals_map[y,x,1] = pcd_normals[n,1]
surface_normals_map[y,x,2] = pcd_normals[n,2]
## Compute standard deviation of local normals
mean_std_norms = np.mean(stdev_filter(surface_normals_map, 25), axis=2)
affordance_map = 1 - mean_std_norms/mean_std_norms.max()
affordance_map[~depth_valid] = 0
return surface_normals_map, affordance_map
def estimate_normals_open3d(self, neighborhood, search_radius):
""" Estimate normals based on PCA, because we cannot vectorize we rely on the c++ open3d backend """
pcd_normal = open3d.PointCloud()
pcd_normal.points = open3d.Vector3dVector(copy.deepcopy(self.points))
open3d.estimate_normals(
pcd_normal,
open3d.KDTreeSearchParamHybrid(radius=search_radius,
max_nn=neighborhood))
self.normals = numpy.asarray(pcd_normal.normals)
return
def visualize_trajectory(numDirs=8, dists=[.1, .3, .5], steps=10):
dino = DinoAgent()
center = dino.center
print("dino is located at: " + str(center))
offset_vals = [-.1, .1]
offsets = [np.array([i, j, k]) for i in offset_vals for j in offset_vals for k in offset_vals] + [np.array([0, 0, 0])]
centers = [np.array(center) + offset for offset in offsets]
ptcloud, sc, start_view = dino.visualize_ptcloud('greedy', 80)
print("start view is: " + str(start_view))
params = getCandidateParams(start_view, numDirs, centers, dists)
print("params: " + str(params))
cand_view_list = [dirToView(d, start_view, c, dist) for (d, c, dist) in params]
cand_traj_pts = cand_view_list
for (d, c, dist) in params:
print("param: " + str((d, c, dist)))
int_pts = interpolateTarget(start_view, d, steps, dist, c)
pts_list = [int_pts.get() for i in range(int_pts.qsize())]
print("points: " + str(pts_list))
cand_traj_pts += pts_list
(greedy_dir, greedy_c, greedy_dist) = chooseGreedyAction(start_view, dists, numDirs, sc, centers)
print("greedy params: d {}, c {}, dist {}".format(greedy_dir, greedy_c, greedy_dist))
greedy_traj = interpolateTarget(start_view, greedy_dir, steps, greedy_dist, greedy_c)
greedy_pts_list = [greedy_traj.get() for i in range(greedy_traj.qsize())]
pcd = open3d.geometry.PointCloud()
pcd.points = open3d.Vector3dVector(ptcloud[:, 0:3])
pcd.paint_uniform_color([1,0.706,0])
total_ptcloud = np.vstack((ptcloud[:, 0:3], np.array(cand_traj_pts)[:, 0:3]))
tpcd = open3d.geometry.PointCloud()
tpcd.points = open3d.Vector3dVector(total_ptcloud)
open3d.estimate_normals(tpcd, search_param = open3d.KDTreeSearchParamHybrid(radius = 0.1, max_nn = 100))
open3d.orient_normals_to_align_with_direction(tpcd)
tpcd.paint_uniform_color([1,0.706,0])
greedy_pcd = open3d.geometry.PointCloud()
greedy_pcd.points = open3d.Vector3dVector(np.array(greedy_pts_list)[:, 0:3])
greedy_pcd.paint_uniform_color([0, 1, 1])
traj_pcd = open3d.geometry.PointCloud()
traj_pcd.points = open3d.Vector3dVector(np.array(cand_traj_pts)[:, 0:3])
traj_pcd.paint_uniform_color([1,0.706,0])
open3d.visualization.draw_geometries([pcd, greedy_pcd])
open3d.write_point_cloud("trajectory_viz.ply", tpcd)
def depth_image_to_pointcloud(img, filter = [None, None, None], estimate_normals=True):
depth_image_preprocessed = preprocess_image(img)
mat = depth_image_to_scientific_coordinates(depth_image_preprocessed, flatten=True, filter=filter)
if mat.shape[0] == 0:
print('No points left after filtering.')
return None
mat = scientific_coordinates_to_standard(mat)
print_pointcloud_mat_stats(mat)
pcd = open3d.PointCloud()
pcd.points = open3d.Vector3dVector(mat)
if estimate_normals:
open3d.estimate_normals(pcd, search_param=open3d.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))
return pcd
def draw_normal(point_cloud):
"""
:param point_cloud:
:return:
"""
if isinstance(point_cloud, PointCloud):
tmp = point_cloud
source = o3d.PointCloud()
source.points = o3d.Vector3dVector(tmp.position)
result = o3d.estimate_normals(
source, o3d.KDTreeSearchParamHybrid(radius=50, max_nn=9))
print('result is', result, 'result type is', type(result))
def load_pcd(data_path, cat):
# load meshes
ply_path = os.path.join(data_path, 'meshes', 'obj_' + cat + '.ply')
model_vsd = ply_loader.load_ply(ply_path)
pcd_model = open3d.PointCloud()
pcd_model.points = open3d.Vector3dVector(model_vsd['pts'])
open3d.estimate_normals(pcd_model,
search_param=open3d.KDTreeSearchParamHybrid(
radius=0.1, max_nn=30))
# open3d.draw_geometries([pcd_model])
model_vsd_mm = copy.deepcopy(model_vsd)
model_vsd_mm['pts'] = model_vsd_mm['pts'] * 1000.0
#pcd_model = open3d.read_point_cloud(ply_path)
return pcd_model, model_vsd, model_vsd_mm
def merge_pointclouds(pcd1, pcd2, keep_colors=True, estimate_normals=True):
if pcd1 is None and pcd2 is None:
return None
if pcd1 is None:
return pcd2
if pcd2 is None:
return pcd1
pcd = open3d.PointCloud()
pcd.points = open3d.Vector3dVector(np.vstack((np.asarray(pcd1.points), np.asarray(pcd2.points))))
if keep_colors:
pcd.colors = open3d.Vector3dVector(np.vstack((np.asarray(pcd1.colors), np.asarray(pcd2.colors))))
if estimate_normals:
open3d.estimate_normals(pcd, search_param=open3d.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))
return pcd
def registration_point_to_plane(scene1, scene2, voxel_size):
# scene1 and 2 are point cloud data
# voxel_size is grid size
#draw_registration_result(scene1,scene2,np.identity(4))
# voxel down sampling
scene1_down = open3d.voxel_down_sample(scene1, voxel_size)
scene2_down = open3d.voxel_down_sample(scene2, voxel_size)
#draw_registration_result(scene1_down,scene2_down,np.identity(4))
# estimate normal with search radius voxel_size*2.0
radius_normal = voxel_size * 2.0
open3d.estimate_normals(
scene1, open3d.KDTreeSearchParamHybrid(radius=radius_normal,
max_nn=30))
open3d.estimate_normals(
scene2, open3d.KDTreeSearchParamHybrid(radius=radius_normal,
max_nn=30))
open3d.estimate_normals(
scene1_down,
open3d.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30))
open3d.estimate_normals(
scene2_down,
open3d.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30))
# compute fpfh feature with search radius voxel_size/2.0
radius_feature = voxel_size * 2.0
scene1_fpfh = open3d.compute_fpfh_feature(
scene1_down,
search_param=open3d.KDTreeSearchParamHybrid(radius=radius_feature,
max_nn=100))
scene2_fpfh = open3d.compute_fpfh_feature(
scene2_down,
search_param=open3d.KDTreeSearchParamHybrid(radius=radius_feature,
max_nn=100))
# compute ransac registration
ransac_result = execute_global_registration(scene1_down, scene2_down,
scene1_fpfh, scene2_fpfh,
voxel_size)
#draw_registration_result(scene1,scene2,ransac_result.transformation)
# point to point ICP resigtration
distance_threshold = voxel_size * 10.0
result = open3d.registration_icp(
scene1, scene2, distance_threshold, ransac_result.transformation,
open3d.TransformationEstimationPointToPlane(),
open3d.ICPConvergenceCriteria(max_iteration=1000))
#draw_registration_result(scene1,scene2,result.transformation)
print(result)
return result
def load_pcd(cat):
# load meshes
mesh_path = "/home/sthalham/data/Meshes/linemod_13/"
#mesh_path = "/home/stefan/data/val_linemod_cc_rgb/models_ply/"
ply_path = mesh_path + 'obj_' + cat + '.ply'
model_vsd = ply_loader.load_ply(ply_path)
pcd_model = open3d.PointCloud()
pcd_model.points = open3d.Vector3dVector(model_vsd['pts'])
open3d.estimate_normals(pcd_model, search_param=open3d.KDTreeSearchParamHybrid(
radius=0.1, max_nn=30))
# open3d.draw_geometries([pcd_model])
model_vsd_mm = copy.deepcopy(model_vsd)
model_vsd_mm['pts'] = model_vsd_mm['pts'] * 1000.0
pcd_model = open3d.read_point_cloud(ply_path)
return pcd_model, model_vsd, model_vsd_mm
def estimate_normals_open3d(pcd, k=1):
dummy = copy.deepcopy(pcd)
mean_nn = pyreg.functions.get_mean_nn_distance(dummy)
voxel_size = k * mean_nn
pcd_open3d = open3d.PointCloud()
pcd_open3d.points = open3d.Vector3dVector(dummy.points)
pcd_open3d = open3d.voxel_down_sample(pcd_open3d, voxel_size=voxel_size)
radius_normal = 2 * voxel_size
open3d.estimate_normals(
pcd_open3d,
open3d.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30))
pcd = PointCloud(points=numpy.asarray(pcd_open3d.points),
normals=numpy.asarray(pcd_open3d.normals))
return pcd
def load_pcd(cat):
# load meshes
#mesh_path = "/home/sthalham/data/LINEMOD/models/"
mesh_path = "/home/stefan/data/Meshes/ycb_video/models/"
template = '000000'
lencat = len(cat)
cat = template[:-lencat] + cat
ply_path = mesh_path + 'obj_' + cat + '.ply'
model_vsd = ply_loader.load_ply(ply_path)
pcd_model = open3d.PointCloud()
pcd_model.points = open3d.Vector3dVector(model_vsd['pts'])
open3d.estimate_normals(pcd_model, search_param=open3d.KDTreeSearchParamHybrid(
radius=20.0, max_nn=30))
# open3d.draw_geometries([pcd_model])
model_vsd_mm = copy.deepcopy(model_vsd)
model_vsd_mm['pts'] = model_vsd_mm['pts'] * 1000.0
pcd_model = open3d.read_point_cloud(ply_path)
return pcd_model, model_vsd, model_vsd_mm
def extract_fpfh(self, radius, max_nn):
"""
Extract the FastPointFeatureHistograms of a point cloud with Open3D
args:
radius: radius for nn-search. Use a much higher radius than for normal computation! e.g. 10xmean_nn
max_nn: max number of nearest neighbors within the radius. Use a higher value as well! e.g. 100
"""
pcd_open3d = open3d.PointCloud()
pcd_open3d.points = open3d.Vector3dVector(self.points)
if self.has_normals() == True:
pcd_open3d.normals = open3d.Vector3dVector(self.normals)
else:
raise Exception("Compute normals before computing FPFH!")
fpfh = open3d.compute_fpfh_feature(
pcd_open3d,
open3d.KDTreeSearchParamHybrid(radius=radius, max_nn=max_nn))
self.fpfh = numpy.asarray(fpfh.data).T
return
