def NonFirstCloud(self, cloud_index):
self.cloud1 = o3d.read_point_cloud(
r"C:\Users\newsmart\Desktop\NSTAlgo\3D\PCL\stitch\2\2-{}_Extract.ply".format(cloud_index - 1))
self.cloud2 = o3d.read_point_cloud(
r"C:\Users\newsmart\Desktop\NSTAlgo\3D\PCL\stitch\2\2-{}_Extract.ply".format(cloud_index))
# get local transformation between two lastest clouds
self.posLocalTrans = self.registerLocalCloud(self.cloud1, self.cloud2)
# if result is not good, drop it
if self.goodResultFlag:
# test for loop detection info
self.detectTransLoop = np.dot(self.posLocalTrans, self.detectTransLoop)
# print ("==== loop detect trans ====")
# print(self.detectTransLoop)
# print ("==== ==== ==== ==== ==== ====")
self.posWorldTrans = np.dot(self.posWorldTrans, self.posLocalTrans)
# update latest cloud
self.cloud1 = copy.deepcopy(self.cloud2)
self.cloud2.transform(self.posWorldTrans)
o3d.write_point_cloud(r"C:\Users\newsmart\Desktop\NSTAlgo\3D\PCL\stitch\2\test.ply",
self.cloud2, write_ascii=False)
self.cloud_base = self.cloud_base + self.cloud2
# Down Sampling
self.cloud_base = o3d.voxel_down_sample(self.cloud_base, 0.001)
self.registrationCount += 1
# save PCD file to local
o3d.write_point_cloud(r"C:\Users\newsmart\Desktop\NSTAlgo\3D\PCL\stitch\2\registerResult.ply",
self.cloud_base, write_ascii=False)
def assignYslice(ySlice, deckTop, p2, ny, zMax, zMin, write):
pierArea = []
deckArea = []
#don't forget deckX contains all the x slices of the deck
red = np.diag(np.divide([255, 0, 0],255))
blue = np.diag(np.divide([0, 0, 255],255))
#deck top is in deckTop[i][row,3] and should correspond to ySlice[i][:][row,3]
for k in range(len(ySlice)):#42 x slices
#note, using the whole x-slice zmax as the local zmax is not##################################################
#entirely accurate. If issues arise, consider finding
#the local zmax for each box seperately
localZmax = np.max(deckTop[k][:,2])
for i in range(ny):#20 y slices per x slice
pcd_export = open3d.PointCloud()
pcd_export.points = open3d.Vector3dVector(ySlice[k][i])
localZmin = np.min(ySlice[k][i][:,2])
#localZmax = np.max(ySlice[k][i][:,2])
if (localZmax-localZmin)>p2*(zMax-zMin):
rgb = np.matmul(np.ones((len(ySlice[k][i]),3)),blue)
pierArea.append(ySlice[k][i])
filename = "../data/step3/nDslice_" + str(len(pierArea)-1) + ".pcd"
else:
rgb = np.matmul(np.ones((len(ySlice[k][i]),3)),red)
deckArea.append(ySlice[k][i])
filename = "../data/step3/Dslice" + str(len(deckArea)-1) + ".pcd"
if write:
pcd_export.colors = open3d.Vector3dVector(rgb)
open3d.write_point_cloud(filename, pcd_export)
return pierArea, deckArea
def input_preprocess(data_dir, id, save_dir):
# rgbd -> point cloud
# start_time = time.time()
pcd = rgbd_to_point_cloud(data_dir, id)
# print("rgbd->pcd: ", time.time() - start_time)
# calculate local normal for point cloud
cal_local_normal(pcd)
# print("cal normal: ", time.time() - start_time)
# select referenced points (default number 2048)
ref_pcd = select_referenced_point(pcd)
# print("select ref: ", time.time() - start_time)
ref_pts = np.asarray(ref_pcd.points)
# assert ref_pts.shape[0] == 2048
# collect local patch for each reference point
neighbor = collect_local_neighbor(ref_pcd, pcd)
# print("collect neighbor:", time.time() - start_time)
# assert len(neighbor) == ref_pts.shape[0]
# calculate the point pair feature for each patch
local_patch = build_local_patch(ref_pcd, pcd, neighbor)
# print("cal ppf for each pair:", time.time() - start_time)
# save the local_patch and reference point cloud for one point cloud fragment.
if not os.path.exists(save_dir):
os.makedirs(save_dir)
np.save(f'{save_dir}/{id}.npy', local_patch.astype(np.float32))
open3d.write_point_cloud(f"{save_dir}/{id}.pcd", ref_pcd)
def save(self, color, depth, mask, img_disp, obj_bboxes, obj_3d_centers, cloud):
self.idx += 1
print "saveing {:05d}th data to file ... ".format(self.idx)
index = int2str(self.idx, 5) + "_"
# Write images
cv2.imwrite(self.fsimage + "/" + index + self.simage + ".png", color)
cv2.imwrite(self.fsdepth + "/" + index + self.sdepth + ".png", depth)
cv2.imwrite(self.fsmask + "/" + index + self.smask + ".png", mask)
cv2.imwrite(self.fsresimg + "/" + index + self.sresimg + ".png", img_disp)
# Write bounding box and other info
with open(self.fsobjects + "/" + index + self.sobjects + ".txt", 'w') as f:
for i in range(len(obj_bboxes)):
# object index
f.write(str(i))
# image size
f.write(": " + str(color.shape[0:2]))
# bounding box
f.write(": " + obj_bboxes[i].converToStr())
# object 3d centers
f.write(": " + str(obj_3d_centers[i]))
f.write("\n")
# Write cloud data
open3d.write_point_cloud(
self.fsclouds + "/" + index + self.sclouds + ".pcd", cloud)
return
def down_sample(dense_pcd_path, dense_label_path, sparse_pcd_path,
sparse_label_path, voxel_size):
# Skip if done
if os.path.isfile(sparse_pcd_path) and (
not os.path.isfile(dense_label_path)
or os.path.isfile(sparse_label_path)):
print("Skipped:", file_prefix)
return
else:
print("Processing:", file_prefix)
# Inputs
dense_pcd = open3d.read_point_cloud(dense_pcd_path)
try:
dense_labels = load_labels(dense_label_path)
except:
dense_labels = None
# Skip label 0, we use explicit frees to reduce memory usage
print("Num points:", np.asarray(dense_pcd.points).shape[0])
if dense_labels is not None:
non_zero_indexes = dense_labels != 0
dense_points = np.asarray(dense_pcd.points)[non_zero_indexes]
dense_pcd.points = open3d.Vector3dVector()
dense_pcd.points = open3d.Vector3dVector(dense_points)
del dense_points
dense_colors = np.asarray(dense_pcd.colors)[non_zero_indexes]
dense_pcd.colors = open3d.Vector3dVector()
dense_pcd.colors = open3d.Vector3dVector(dense_colors)
del dense_colors
dense_labels = dense_labels[non_zero_indexes]
print("Num points after 0-skip:",
np.asarray(dense_pcd.points).shape[0])
# Downsample points
min_bound = dense_pcd.get_min_bound() - voxel_size * 0.5
max_bound = dense_pcd.get_max_bound() + voxel_size * 0.5
sparse_pcd, cubics_ids = open3d.voxel_down_sample_and_trace(
dense_pcd, voxel_size, min_bound, max_bound, False)
print("Num points after down sampling:",
np.asarray(sparse_pcd.points).shape[0])
open3d.write_point_cloud(sparse_pcd_path, sparse_pcd)
print("Point cloud written to:", sparse_pcd_path)
# Downsample labels
if dense_labels is not None:
sparse_labels = []
for cubic_ids in cubics_ids:
cubic_ids = cubic_ids[cubic_ids != -1]
cubic_labels = dense_labels[cubic_ids]
sparse_labels.append(np.bincount(cubic_labels).argmax())
sparse_labels = np.array(sparse_labels)
write_labels(sparse_label_path, sparse_labels)
print("Labels written to:", sparse_label_path)
def cb_save(msg):
global Model,tfReg
#save point cloud
for n,l in enumerate(Config["scenes"]):
if Scene[n] is None: continue
pc=o3d.PointCloud()
m=Scene[n]
Model[n]=m
pc.points=o3d.Vector3dVector(m)
o3d.write_point_cloud(Config["path"]+"/"+l+".ply",pc,True,False)
pub_pcs[n].publish(np2F(m))
tfReg=[]
#copy TF scene...->master... and save them
for n,s in enumerate(Config["scene_frame_id"]):
try:
tf=tfBuffer.lookup_transform("world",s,rospy.Time())
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
pub_msg.publish("searcher::capture::lookup failure world->"+s)
tf=TransformStamped()
tf.header.stamp=rospy.Time.now()
tf.header.frame_id="world"
tf.child_frame_id=s
tf.transform.rotation.w=1
path=Config["path"]+"/"+Config["master_frame_id"][n].replace('/','_')+".yaml"
f=open(path,"w")
f.write(yaml.dump(tflib.tf2dict(tf.transform)))
f.close()
tfReg.append(tf)
broadcaster.sendTransform(tfReg)
solver.learn(Model,Param)
pub_msg.publish("searcher::model learning completed")
def removeDeckTop(notDeckX, write):
deckTop = []
red = np.diag(np.divide([255, 0, 0],255))
blue = np.diag(np.divide([0, 0, 255],255))
for i in range(len(notDeckX)):
#print("Length slice before: " + str(len(notDeckX[i])))
yMin = np.min(notDeckX[i][:,1])
yMax = np.max(notDeckX[i][:,1])
zMax = np.max(notDeckX[i][:,2])
deltaY = (yMax-yMin)
deltaZ = 0.05*deltaY
notDeckX[i] = notDeckX[i][np.argsort(notDeckX[i][:,2]), :]
BR = bisect.bisect_left(notDeckX[i][:,2],zMax-deltaZ)
deckTop.append(notDeckX[i][BR:,:])
notDeckX[i] = notDeckX[i][:BR,:]
#print("yMin %.3f\t yMax %.3f\t deltaY %.3f\t deltaZ %.3f\t BR %.3f\t"%(yMin,yMax,deltaY,deltaZ,BR))
#print("Length slice after: " + str(len(notDeckX[i])))
if write:
filename = "../data/step2_5/Dtop_" + str(len(deckTop)-1) + ".pcd"
pcd_export = open3d.PointCloud()
pcd_export.points = open3d.Vector3dVector(deckTop[i])
rgb = np.matmul(np.ones((len(deckTop[i]),3)),red)
pcd_export.colors = open3d.Vector3dVector(rgb)
open3d.write_point_cloud(filename, pcd_export)
filename = "../data/step2_5/Parea_" + str(i) + ".pcd"
pcd_export = open3d.PointCloud()
pcd_export.points = open3d.Vector3dVector(notDeckX[i])
rgb = np.matmul(np.ones((len(notDeckX[i]),3)),blue)
pcd_export.colors = open3d.Vector3dVector(rgb)
open3d.write_point_cloud(filename, pcd_export)
return notDeckX, deckTop
def savePCs(filename,pcs,pc):
'''
saves a merged point cloud and also the separated pointclouds
Args:
filename String: what should the name be
pcs [open3d.pointcloud]: list of all the retrieved pointclouds
'''
global saveInc
saveInc=saveInc+1
print(saveInc)
print(filename)
filename = filename+"_"+str(saveInc)
#creates a file for the merged pointcloud
open3d.write_point_cloud("./PC/"+filename+".ply", pc)
#create a folder for the unmerged pointclouds
try:
os.mkdir("./PC/"+filename)
except:
print("PA")
#save the individual pointclouds
for i in range(len(pcs)):
print(pcs[i])
open3d.write_point_cloud("./PC/"+filename+"/pointcloud"+str(i)+".ply", pcs[i])
def cb_save_ply(msg):
d = outPn.astype(np.float32)
pc = o3d.PointCloud()
pc.points = o3d.Vector3dVector(d)
print 'save model PC', d.dtype, d.shape
o3d.write_point_cloud(Args["wd"] + "/sample.ply", pc, True, False)
return
def point_cloud_txt_to_pcd(raw_dir, file_prefix):
# File names
txt_file = os.path.join(raw_dir, file_prefix + ".txt")
pts_file = os.path.join(raw_dir, file_prefix + ".pts")
pcd_file = os.path.join(raw_dir, file_prefix + ".pcd")
# Skip if already done
if os.path.isfile(pcd_file):
print("pcd {} exists, skipped".format(pcd_file))
return
# .txt to .pts
# We could just prepend the line count, however, there are some intensity value
# which are non-integers.
print("[txt->pts]")
print("txt: {}".format(txt_file))
print("pts: {}".format(pts_file))
with open(txt_file, "r") as txt_f, open(pts_file, "w") as pts_f:
for line in txt_f:
# x, y, z, i, r, g, b
tokens = line.split()
tokens[3] = str(int(float(tokens[3])))
line = " ".join(tokens)
pts_f.write(line + "\n")
prepend_line(pts_file, str(wc(txt_file)))
# .pts -> .pcd
print("[pts->pcd]")
print("pts: {}".format(pts_file))
print("pcd: {}".format(pcd_file))
point_cloud = open3d.read_point_cloud(pts_file)
open3d.write_point_cloud(pcd_file, point_cloud)
def convert(input_dir,
obj_file_name_1,
mtl_file_name_1,
obj_file_name_2,
mtl_file_name_2,
ply_file_name,
n,
nosf=False,
write_ascii=True,
u=None,
v=None,
random_indices=None):
materials_1 = parse_mtl(mtl_file_name_1)
vertices_1, faces_1, textures_1, parts_1 = parse_obj(
input_dir, obj_file_name_1, materials_1)
materials_2 = parse_mtl(mtl_file_name_2)
vertices_2, faces_2, textures_2, parts_2 = parse_obj(
input_dir, obj_file_name_2, materials_2)
if u is None or v is None or random_indices is None:
points_1, colors_1, random_indices, u, v = sample(
vertices_1, faces_1, textures_1, parts_1, n, input_dir,
materials_1)
else:
points_1, colors_1, random_indices, u, v = sample(
vertices_1,
faces_1,
textures_1,
parts_1,
n,
input_dir,
materials_1,
u=u,
v=v,
random_indices=random_indices)
points_2, colors_2, random_indices, u, v = sample(
vertices_2,
faces_2,
textures_2,
parts_2,
n,
input_dir,
materials_2,
random_indices=random_indices,
u=u,
v=v)
flow_xyz = points_2 - points_1
pc = open3d.PointCloud()
pc.points = open3d.Vector3dVector(points_1)
pc.colors = open3d.Vector3dVector(colors_1)
if not nosf:
pc.normals = open3d.Vector3dVector(flow_xyz)
open3d.write_point_cloud(ply_file_name, pc, write_ascii=write_ascii)
# open3d.draw_geometries([pc])
return random_indices, u, v
def export(self,k,color):
#print("I'm not done yet")
filename = "../data/elements/cluster" + str(k) + "_" + str(self.x) + "," + str(self.y) + ".pcd"
pcd_export = open3d.PointCloud()
pcd_export.points = open3d.Vector3dVector(self.points)
rgb = np.matmul(np.ones((len(self.points),3)),np.diag(color))
pcd_export.colors = open3d.Vector3dVector(rgb)
open3d.write_point_cloud(filename, pcd_export)
def color_deviations(config, result, distances):
""" Use the Open3d visualization function to colorize deviations after convergence """
if isinstance(result, PointCloud) == True:
result = open3d.PointCloud()
result.points = open3d.Vector3dVector(result.points)
elif isinstance(result, numpy.ndarray) == True:
result = open3d.PointCloud()
result.points = open3d.Vector3dVector(result)
elif isinstance(result, open3d.PointCloud) == True:
pass
else:
raise TypeError(
"Point clouds need to be of type (ndarray), (PointCloud) or (open3d.PointCloud)!"
)
# Needed Paths
OPEN3D_BUILD_PATH = "/home/chris/Documents/Masterarbeit/Code/virt_env/env/lib/python3.6/site-packages/Open3D/build/"
OPEN3D_EXPERIMENTAL_BIN_PATH = OPEN3D_BUILD_PATH + "/bin/Experimental/"
# TODO use os.findpath or something to make this dynamic
DATASET_DIR = "/home/chris/Documents/Masterarbeit/Code/virt_env/env/ICP/src/bin"
# Log files
MY_LOG_POSTFIX = "_COLMAP_SfM.log"
MY_RECONSTRUCTION_POSTFIX = "_COLMAP.ply"
# File structure
mvs_outpath = DATASET_DIR + config["colorize_deviations"]["output_path"]
"/../../data/set1/output/evaluation/"
scene = config["colorize_deviations"]["scene"] + "/"
# Make directory
make_dir(mvs_outpath)
make_dir(mvs_outpath + "/" + scene)
# New log files
new_logfile = DATASET_DIR + MY_LOG_POSTFIX
colmap_ref_logfile = DATASET_DIR + scene + "_COLMAP_SfM.log"
mvs_file = mvs_outpath + scene + MY_RECONSTRUCTION_POSTFIX
# Write the distances to bin files
numpy.array(distances).astype("float64").tofile(mvs_outpath + "/" + scene +
".precision.bin")
# Colorize the poincloud files with the precision and recall values
open3d.write_point_cloud(mvs_outpath + "/" + scene + ".precision.ply",
result)
open3d.write_point_cloud(mvs_outpath + "/" + scene + ".precision.ncb.ply",
result)
result_n_fn = mvs_outpath + "/" + scene + ".precision.ply"
eval_str_viewDT = OPEN3D_EXPERIMENTAL_BIN_PATH + "ViewDistances " + result_n_fn + " --max_distance " + str(
numpy.asarray(distances).max()) + " --write_color_back --without_gui"
os.system(eval_str_viewDT)
result_colored = open3d.read_point_cloud(result_n_fn)
open3d.draw_geometries([result_colored])
return
def write_to_point_cloud(data, colors, idx):
points = np.asarray(data, dtype=np.float32)
colors = np.asarray(colors, dtype=np.float32)
cloud = o3.PointCloud()
cloud.points = o3.Vector3dVector(points)
cloud.colors = o3.Vector3dVector(colors)
print(f'writing to pointcloud{idx}.pcd')
o3.write_point_cloud(f'pointcloud{idx}.pcd', cloud)
o3.write_point_cloud(f'pointcloud{idx}.xyzrgb', cloud)
def points_save(points, colors, root='pcds/regions', child='all', pfile=''):
os.makedirs(root, exist_ok=True)
os.makedirs(root + '/' + child, exist_ok=True)
pcd = o3d.PointCloud()
pcd.points = o3d.Vector3dVector(np.float32(points))
pcd.colors = o3d.Vector3dVector(np.float32(colors))
o3d.write_point_cloud(os.path.join(root, '%s.pcd' % pfile),
pcd,
write_ascii=True,
compressed=True)
def ply2xyz(self, FN):
fileDir = "C:/Users/Ozguc Capunaman/Documents/GitHub/InteractiveDigitalFabrication/sessions/" + self._session
srcDir = fileDir + "/" + FN + ".ply"
ply = open3d.read_triangle_mesh(srcDir)
xyz = open3d.PointCloud()
srcDir = fileDir + "/" + FN
xyz.points = ply.vertices
targetDir = fileDir + "/" + FN + ".xyz"
open3d.write_point_cloud(targetDir, xyz)
def on_lidar_frame(self, msg):
self._pc_msg_cnt += 1
if self._pc_msg_cnt % self._flags.log_every_nth_frame != 0:
return
# Write the lidar information.
file_name = '{}carla-lidar-{}.ply'.format(
self._flags.data_path, msg.timestamp.coordinates[0])
pcd = o3d.PointCloud()
pcd.points = o3d.Vector3dVector(msg.point_cloud)
o3d.write_point_cloud(file_name, pcd)
def ply2xyz(filename="", srcPath="../ply/", targetPath="../xyz/"):
srcDir = srcPath + filename + ".ply"
ply = open3d.read_triangle_mesh(srcDir)
xyz = open3d.PointCloud()
xyz.points = ply.vertices
targetDir = targetPath + filename + ".xyz"
print(targetDir)
open3d.write_point_cloud(targetDir, xyz)
return xyz
def postprocess(pc_path, json_path, r):
# find rotation of the point cloud, so it's alligned with z axis
pcd = o3d.read_point_cloud(pc_path)
# find transformation of the point cloud, that will allign it with x axis
pcd1 = copy.deepcopy(pcd)
rot_trans_straight1 = su.calc_centre_correction(pcd1)
pcd1.transform(rot_trans_straight1)
rot_trans_straight2 = su.calc_centre_correction(pcd1)
pcd1.transform(rot_trans_straight2)
# do this twice and combine results
rot_trans_straight = np.dot(rot_trans_straight2, rot_trans_straight1)
# make the points start from z=0
points = np.asarray(pcd.points)
z_trans = min(points[:, 2])
rot_trans_straight [2, 3] -= z_trans
pcd.transform(rot_trans_straight)
# transform 2d np array to 2d list
l_transformation = [list(r) for r in rot_trans_straight]
print(l_transformation)
# append transformation to the end of json file
with open(json_path, mode='r', encoding='utf-8') as feedsjson:
feeds = json.load(feedsjson)
with open(json_path, mode='w', encoding='utf-8') as feedsjson:
entry = {'postprocessing_transformation': l_transformation}
feeds.append(entry)
json.dump(feeds, feedsjson)
output_folder, _ = split(json_path)
o3d.write_point_cloud(join(output_folder, 'stitched_straight.pcd'), pcd)
points = np.asarray(pcd.points)
# points to flat surface
points = cart2pol_pc(points, r)
# save points in the point cloud
pcd.points = o3d.Vector3dVector(points)
# find normals so it's displayed nicer
# o3d.estimate_normals(pcd, search_param = o3d.KDTreeSearchParamHybrid(
# radius = 5, max_nn = 30))
# o3d.draw_geometries([pcd])
output_path = join(output_folder, 'flatten.pcd')
o3d.write_point_cloud(output_path, pcd)
return output_path
def main():
args = load_args()
input_x = sorted(os.listdir(args.x))
input_y = sorted(os.listdir(args.y))
for x in input_x:
x = split(x, '.')[0]
for y in input_y:
y = split(y, '.')[0]
if x == y:
o3d.write_point_cloud(args.o + x, x)
o3d.write_point_cloud(args.o + y, y)
def get_grasp_solution(self, req):
resp = GraspSolutionResponse()
if self.grasp_generator.has_solutions():
solution = self.grasp_generator.get_solution(self.sidx)
resp.joint_angles = solution.joint_angles
resp.base_pose = list(solution.base_pose[0]) + list(
solution.base_pose[1])
for i in range(len(solution.joint_trajectory)):
grasp_waypoint = GraspWaypoint()
grasp_waypoint.base_pose = solution.base_trajectory[
i].p.tolist() + solution.base_trajectory[i].q.tolist()
grasp_waypoint.joint_angles = solution.joint_trajectory[
i].tolist()
resp.trajectory.waypoints.append(grasp_waypoint)
#lift waypoint
grasp_waypoint = GraspWaypoint()
grasp_waypoint.joint_angles = solution.joint_trajectory[-1].tolist(
)
grasp_waypoint.base_pose = solution.lift_base_pose.p.tolist(
) + solution.lift_base_pose.q.tolist()
resp.trajectory.waypoints.append(grasp_waypoint)
resp.cloud_min_pt = np.amin(self.point_cloud.points(), axis=0)
resp.cloud_max_pt = np.amax(self.point_cloud.points(), axis=0)
resp.cloud_centroid = self.point_cloud.compute_centroid()
if SAVE_DATA:
import open3d as o3d
self.cloud_name = raw_input("Object name: ")
models_path = get_aml_package_path('aml_data')
filepath = models_path + '/exp_data/' + self.cloud_name + '.pcd'
o3d.write_point_cloud(filepath, self.point_cloud._cloud, True)
print "Cloud data saved to: ", filepath
models_path = get_aml_package_path('aml_data')
filepath = models_path + '/exp_data/' + self.cloud_name + "_grasps.pkl"
save_data(
{
'solution': solution,
'sidx': self.sidx,
'solution_list': self.grasp_generator.get_solutions()
}, filepath)
print "Solution data saved to: ", filepath
pos = resp.base_pose[0:3]
quat = resp.base_pose[3:7]
self.br.sendTransform(pos, quat, rospy.Time.now(),
"/target/j2n6s300_link_6", "world")
return resp
def process_single_fragment(cfg, color_files, depth_files, frag_id, n_frags,
intrinsic_path, out_folder):
import open3d as o3d
depth_only_flag = (len(color_files) == 0)
n_frames = len(depth_files)
intrinsic = read_intrinsic(intrinsic_path, cfg.width, cfg.height)
if depth_only_flag:
color_type = o3d.integration.TSDFVolumeColorType.__dict__['None']
else:
color_type = o3d.integration.TSDFVolumeColorType.__dict__['RGB8']
volume = o3d.integration.ScalableTSDFVolume(
voxel_length=cfg.tsdf_cubic_size / 512.0,
sdf_trunc=0.04,
color_type=color_type)
sid = frag_id * cfg.frames_per_frag
eid = min(sid + cfg.frames_per_frag, n_frames)
pose_base2world = None
pose_base2world_inv = None
for fid in range(sid, eid):
if not depth_only_flag:
color_path = color_files[fid]
else:
color_path = None
depth_path = depth_files[fid]
pose_path = depth_path[:-10] + '.pose.txt'
pose_cam2world = read_extrinsic(pose_path)
if pose_cam2world is None:
continue
if fid == sid: # Use as base frame
pose_base2world = pose_cam2world
pose_base2world_inv = np.linalg.inv(pose_base2world)
if pose_base2world_inv is None:
break
# Relative camera pose
pose_cam2world = np.matmul(pose_base2world_inv, pose_cam2world)
rgbd = read_rgbd_image(cfg, color_path, depth_path, False)
volume.integrate(rgbd, intrinsic, np.linalg.inv(pose_cam2world))
if pose_base2world_inv is None:
return
pcloud = volume.extract_point_cloud()
o3d.geometry.estimate_normals(pcloud)
o3d.write_point_cloud(
os.path.join(out_folder, 'cloud_bin_{}.ply'.format(frag_id)), pcloud)
np.save(os.path.join(out_folder, 'cloud_bin_{}.pose.npy'.format(frag_id)),
pose_base2world)
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 colorize(input_pcd_path, input_labels_path, output_pcd_path):
pcd = open3d.read_point_cloud(input_pcd_path)
print("Point cloud loaded from", input_pcd_path)
labels = load_labels(input_labels_path)
print("Labels loaded from", input_labels_path)
s = time.time()
colorize_point_cloud(pcd, labels)
print("time colorize_point_cloud pd", time.time() - s, flush=True)
open3d.write_point_cloud(output_pcd_path, pcd)
print("Output written to", output_pcd_path)
def main(radius, name):
cloud = epc.compress((epc[:, -2] > radius) & (radius >= epc[:, -1]), 0)
pcd = PointCloud()
pcd.points = Vector3dVector(cloud[:, :3])
pcd.colors = Vector3dVector(cloud[:, 3:6])
write_point_cloud(name, pcd)
print(
pcd,
cloud[:, -1].max(),
cloud[:, -3].sum(),
cloud[:, -4].max(),
cloud[:, -5].sum(),
)
return None
def save(self, timestamp: int, data_path: str, file_base: str):
"""Saves the point cloud to a file.
Args:
timestamp (:obj:`int`): Timestamp associated with the point cloud.
data_path (:obj:`str`): Path where to save the point cloud.
file_base (:obj:`str`): Base name of the file.
"""
import open3d as o3d
file_name = os.path.join(data_path,
'{}-{}.ply'.format(file_base, timestamp))
pcd = o3d.PointCloud()
pcd.points = o3d.Vector3dVector(self.points)
o3d.write_point_cloud(file_name, pcd)
def compare(parameter):
picindex1 = 000
picindex2 = 111
print("welcome to compare")
pc1, image1, indexlist1 = parameter['0']
pc2, image2, indexlist2 = parameter['1']
o3d.write_point_cloud("combine.ply", pc1 + pc2, write_ascii=True)
print("image shape", image1.shape)
points1 = np.array(pc1.points)
points2 = np.array(pc2.points)
print("point count:", len(points1))
print("point count:", len(points2))
nbrs = NearestNeighbors(n_neighbors=1, n_jobs=10).fit(points2)
print("neighbor match finish")
distance, indices = nbrs.kneighbors(points1)
del nbrs
#thersh = np.sqrt(distance)
thersh = np.percentile(np.array(distance), 10)
#thersh = 2e-2
#showing
print("thersh is", thersh)
plt.hist(distance, bins=100)
plt.ylabel('Distance of neighbor points' + str(picindex1) + "+" +
str(picindex2))
plt.show()
#match
match = []
print("count ", len(distance))
for i in range(len(distance)):
if points1[i][0] + points1[i][1] + points1[i][2] == 0:
continue
if distance[i] < thersh:
match.append([i, indices[i][0]])
#o3d.visualization.draw_geometries([pc1, pc2])
###watch mid point
################
bs = 3
blockStat(bs, match, indexlist1, indexlist2, image1, image2)
bs = 7
blockStat(bs, match, indexlist1, indexlist2, image1, image2)
bs = 15
blockStat(bs, match, indexlist1, indexlist2, image1, image2)
bs = 31
blockStat(bs, match, indexlist1, indexlist2, image1, image2)
bs = 63
blockStat(bs, match, indexlist1, indexlist2, image1, image2)
return
def save_cam_ply(cam_fn=None, show=False, with_walls=True, with_pcl=True):
if cam_fn is None:
house_name = '0004d52d1aeeb8ae6de39d6bd993e992_A'
parsed_dir = f'{SUNCG_V1_DIR}/parsed/{house_name}'
cam_fn = os.path.join(parsed_dir, 'cam')
cam_fn = '/home/z/parsed/28297783bce682aac7fb35a1f35f68fa/cam'
cam_pos = read_cam_pos(cam_fn)
parsed_dir = os.path.dirname(cam_fn)
cam_num = cam_pos.shape[0]
cam_centroid = cam_pos[:, 0:3]
# cam centroid box
tmp = np.array([[0.1, 0.1, 0.1, 0]])
tmp = np.tile(tmp, [cam_num, 1])
cam_cen_box = cam_pos[:, 0:3]
cam_cen_box = np.concatenate([cam_centroid, tmp], 1)
cam_cen_box = Bbox3D.bboxes_lineset(cam_cen_box,
'Z',
False,
random_color=False)
# cam orientations
cam_ori = cam_centroid + cam_pos[:, 3:6]
cam_centroid = np.expand_dims(cam_centroid, 1)
cam_ori = np.expand_dims(cam_ori, 1)
cam_vec = np.concatenate([cam_centroid, cam_ori], 1)
cam_lines = Bbox3D.draw_lines_open3d(cam_vec,
color=[200, 0, 0],
show=False)
pcl_fn = os.path.dirname(cam_fn) + '/pcl_camref.ply'
if show:
if with_pcl and os.path.exists(pcl_fn):
pcl = open3d.read_point_cloud(pcl_fn)
open3d.draw_geometries(cam_cen_box + [cam_lines, pcl])
if with_walls:
bbox_fn = f'{parsed_dir}/object_bbox/wall.txt'
walls = np.loadtxt(bbox_fn).reshape([-1, 7])
walls = world2cam_box(walls)
bboxes_lineset_ls = Bbox3D.bboxes_lineset(walls, 'Y', False)
open3d.draw_geometries(cam_cen_box + [cam_lines] +
bboxes_lineset_ls)
# save cam pos as ply
pcd = open3d.PointCloud()
pcd.points = open3d.Vector3dVector(cam_pos[:, 0:3])
cam_ply_fn = cam_fn + '_pos.ply'
open3d.write_point_cloud(cam_ply_fn, pcd)
def main_save_cloud():
global received_ros_cloud
rate = rospy.Rate(100)
cnt = 0
file_folder = "/home/feiyu/baxterws/src/winter_prj/mask_objects_from_rgbd/data/"
file_name = "pcd_"
while not rospy.is_shutdown():
if received_ros_cloud is not None:
cnt += 1
open3d_cloud = convertCloudFromRosToOpen3d(received_ros_cloud)
filename_to_save = file_folder + file_name + "{:03d}".format(
cnt) + ".pcd"
open3d.write_point_cloud(filename_to_save, open3d_cloud)
print("Saving" + filename_to_save)
received_ros_cloud = None # clear
rate.sleep()
def main():
f_depth_x = 1.0580353759137747e+03
f_depth_y = 1.0604471766882732e+03
c_depth_x = 9.5177505245974123e+02
c_depth_y = 5.4839653660364445e+02
depth_path1 = 'Depth/2028.png'
img1_depth = np.array(imread(depth_path1))
camera_matrix = np.array([[1 / f_depth_x, 0., -c_depth_x / f_depth_x],
[0., 1 / f_depth_y, -c_depth_y / f_depth_y],
[0., 0., 1.]])
camera_matrix_opencv = np.array([[f_depth_x, 0., c_depth_x],
[0., f_depth_y, c_depth_y], [0., 0., 1.]])
# pcl1 = pcl_from_open3d(img1_RGB, img1_depth, camera_matrix)
# pcl2 = pcl_from_open3d(img2_RGB, img2_depth, camera_matrix)
pcl1 = pcl_from_depth_image(img1_depth, camera_matrix)
o3d.write_point_cloud("pcl1.ply", pcl1)
