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()
Exemple #3
0
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 = []