if d.get_info(rs.camera_info.name).lower() != 'platform camera':
connect_device.append(d.get_info(rs.camera_info.serial_number))
if len(connect_device) < 2:
print('Registrition needs two camera connected.But got one.')
exit()
pipeline1 = rs.pipeline()
rs_config.enable_device(connect_device[0])
pipeline_profile1 = pipeline1.start(rs_config)
intr1 = pipeline_profile1.get_stream(
rs.stream.color).as_video_stream_profile().get_intrinsics()
pinhole_camera1_intrinsic = o3d.PinholeCameraIntrinsic(
intr1.width, intr1.height, intr1.fx, intr1.fy, intr1.ppx, intr1.ppy)
cam1 = rgbdTools.Camera(intr1.fx, intr1.fy, intr1.ppx, intr1.ppy)
# print('cam1 intrinsics:')
# print(intr1.width, intr1.height, intr1.fx, intr1.fy, intr1.ppx, intr1.ppy)
pipeline2 = rs.pipeline()
rs_config.enable_device(connect_device[1])
pipeline_profile2 = pipeline2.start(rs_config)
intr2 = pipeline_profile2.get_stream(
rs.stream.color).as_video_stream_profile().get_intrinsics()
pinhole_camera2_intrinsic = o3d.PinholeCameraIntrinsic(
intr2.width, intr2.height, intr2.fx, intr2.fy, intr2.ppx, intr2.ppy)
cam2 = rgbdTools.Camera(intr2.fx, intr2.fy, intr2.ppx, intr2.ppy)
# print('cam2 intrinsics:')
# print(intr2.width, intr2.height, intr2.fx, intr2.fy, intr2.ppx, intr2.ppy)
#align = rs.align(rs.stream.depth)
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 15)
config.enable_stream(rs.stream.color, 640, 480, rs.format.rgb8, 15)
pipeline = rs.pipeline()
profile = pipeline.start(config)
# get camera intrinsics
intr = profile.get_stream(
rs.stream.color).as_video_stream_profile().get_intrinsics()
# print(intr.width, intr.height, intr.fx, intr.fy, intr.ppx, intr.ppy)
pinhole_camera_intrinsic = o3d.camera.PinholeCameraIntrinsic(
intr.width, intr.height, intr.fx, intr.fy, intr.ppx, intr.ppy)
RealSense = rgbdTools.Camera(fx=intr.fx,
fy=intr.fy,
cx=intr.ppx,
cy=intr.ppy)
TablePlane = keyPoints.Plane()
# print(type(pinhole_camera_intrinsic))
cv2.namedWindow('Color Stream', cv2.WINDOW_AUTOSIZE)
# cv2.namedWindow('Depth Stream', cv2.WINDOW_AUTOSIZE)
time_start = time.time()
globalPointcloud = o3d.geometry.PointCloud()
Point2 = o3d.geometry.PointCloud()
Point3 = o3d.geometry.PointCloud()
Point4 = o3d.geometry.PointCloud()
prePoint3 = o3d.geometry.PointCloud()
from tools import keyPoints
from tools import registration
import cv2
import numpy as np
from scipy import optimize
import open3d as o3d
import sys
import os
import time
time_start = time.time()
path_to_rgbd = './bear/'
RealSense = rgbdTools.Camera(fx=616.8676,
fy=617.0631,
cx=319.5701,
cy=233.0649)
TablePlane = keyPoints.Plane()
cloud_number = 8
voxel_size = 0.0001
Point2_list = []
Point3_list = []
Point4_list = []
for j in range(cloud_number):
print('Processing No.', j, 'view .')
Point2 = o3d.PointCloud()
Point3 = o3d.PointCloud()
Point4 = o3d.PointCloud()
Color2 = []
Pt2 = []
pipeline = rs.pipeline()
profile = pipeline.start(config)
# get camera intrinsics
intr = profile.get_stream(
rs.stream.color).as_video_stream_profile().get_intrinsics()
# print(intr.width, intr.height, intr.fx, intr.fy, intr.ppx, intr.ppy)
pinhole_camera_intrinsic = PinholeCameraIntrinsic(intr.width, intr.height,
intr.fx, intr.fy,
intr.ppx, intr.ppy)
# print(type(pinhole_camera_intrinsic))
cv2.namedWindow('Color Stream', cv2.WINDOW_AUTOSIZE)
cv2.namedWindow('Depth Stream', cv2.WINDOW_AUTOSIZE)
cam = rgbdTools.Camera(616.8676147460938, 617.0631103515625,
319.57012939453125, 233.06488037109375)
geometrie_added = False
vis = Visualizer()
#vis.create_window("Pointcloud",640,480)
vis.create_window("Pointcloud")
pointcloud = PointCloud()
i = 0
plane_flag = 0
tablePlane = keyPoints.Plane()
z_min = None
z_max = None
x_min = None
x_max = None
from tools import keyPoints
from tools import registration
import cv2
import numpy as np
from scipy import optimize
import open3d as o3d
import sys
import os
import time
time_start = time.time()
path_to_rgbd = './bear/'
RealSense = rgbdTools.Camera(fx=614.2888,
fy=614.3722,
cx=321.6398,
cy=237.2958)
TablePlane = keyPoints.Plane()
cloud_number = 8
voxel_size = 0.0001
Point2_list = []
Point3_list = []
Point4_list = []
for j in range(cloud_number):
print('Processing No.', j, 'view .')
Point2 = o3d.PointCloud()
Point3 = o3d.PointCloud()
Point4 = o3d.PointCloud()
Color2 = []
Pt2 = []