From 4118e032147855b07461638ca2e54e50cfbe556e Mon Sep 17 00:00:00 2001 From: Jaesik Park Date: Fri, 19 Oct 2018 16:05:45 -0700 Subject: [PATCH] adding sensors/realsense_pcd_visualizer.py & polish documents --- .../capture_your_own_dataset.rst | 8 +- .../sensors/realsense_pcd_visualizer.py | 115 ++++++++++++++++++ .../realsense_recorder.py} | 2 +- src/Core/Geometry/PointCloudFactory.cpp | 28 ++++- 4 files changed, 144 insertions(+), 9 deletions(-) create mode 100644 examples/Python/ReconstructionSystem/sensors/realsense_pcd_visualizer.py rename examples/Python/ReconstructionSystem/{recorder_realsense.py => sensors/realsense_recorder.py} (98%) diff --git a/docs/tutorial/ReconstructionSystem/capture_your_own_dataset.rst b/docs/tutorial/ReconstructionSystem/capture_your_own_dataset.rst index 6b1c8113468..454a1f76026 100644 --- a/docs/tutorial/ReconstructionSystem/capture_your_own_dataset.rst +++ b/docs/tutorial/ReconstructionSystem/capture_your_own_dataset.rst @@ -3,7 +3,7 @@ Capture your own dataset ------------------------------------- -If you have a `RealSense camera `_, capturing RGBD frames is easy by using ``recoder_realsense.py``. +If you have a `RealSense camera `_, capturing RGBD frames is easy by using ``sensors/realsense_recoder.py``. .. note:: This tutorial assumes that valid RealSense Python package and OpenCV python package are installed in your environment. Please follow `this instruction `_ to install RealSense Python package. @@ -14,9 +14,9 @@ The script runs with one of the following three options: .. code-block:: bash - python recoder_realsense.py --record_imgs - python recoder_realsense.py --record_rosbag - python recoder_realsense.py --playback_rosbag + python realsense_recoder.py --record_imgs + python realsense_recoder.py --record_rosbag + python realsense_recoder.py --playback_rosbag In either ``record_imgs`` and ``record_rosbag`` mode, the script displays the following capturing preview. diff --git a/examples/Python/ReconstructionSystem/sensors/realsense_pcd_visualizer.py b/examples/Python/ReconstructionSystem/sensors/realsense_pcd_visualizer.py new file mode 100644 index 00000000000..0b2ffc61c6b --- /dev/null +++ b/examples/Python/ReconstructionSystem/sensors/realsense_pcd_visualizer.py @@ -0,0 +1,115 @@ +# pyrealsense2 is required. +# Please see instructions in https://github.com/IntelRealSense/librealsense/tree/master/wrappers/python +import pyrealsense2 as rs +from open3d import * +import numpy as np +from enum import IntEnum + +from datetime import datetime +from open3d import * + +class Preset(IntEnum): + Custom = 0 + Default = 1 + Hand = 2 + HighAccuracy = 3 + HighDensity = 4 + MediumDensity = 5 + +def get_intrinsic_matrix(frame): + intrinsics = frame.profile.as_video_stream_profile().intrinsics + out = PinholeCameraIntrinsic(640, 480, + intrinsics.fx, intrinsics.fy, intrinsics.ppx, intrinsics.ppy) + return out + + +if __name__ == "__main__": + + # Create a pipeline + pipeline = rs.pipeline() + + #Create a config and configure the pipeline to stream + # different resolutions of color and depth streams + config = rs.config() + + config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30) + config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30) + + # Start streaming + profile = pipeline.start(config) + depth_sensor = profile.get_device().first_depth_sensor() + + # Using preset HighAccuracy for recording + depth_sensor.set_option(rs.option.visual_preset, Preset.HighAccuracy) + + # Getting the depth sensor's depth scale (see rs-align example for explanation) + depth_scale = depth_sensor.get_depth_scale() + + # We will not display the background of objects more than + # clipping_distance_in_meters meters away + clipping_distance_in_meters = 3 # 3 meter + clipping_distance = clipping_distance_in_meters / depth_scale + # print(depth_scale) + + # Create an align object + # rs.align allows us to perform alignment of depth frames to others frames + # The "align_to" is the stream type to which we plan to align depth frames. + align_to = rs.stream.color + align = rs.align(align_to) + + vis = Visualizer() + vis.create_window() + + pcd = PointCloud() + flip_transform = [[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]] + + # Streaming loop + frame_count = 0 + try: + while True: + + dt0=datetime.now() + + # Get frameset of color and depth + frames = pipeline.wait_for_frames() + + # Align the depth frame to color frame + aligned_frames = align.process(frames) + + # Get aligned frames + aligned_depth_frame = aligned_frames.get_depth_frame() + color_frame = aligned_frames.get_color_frame() + intrinsic = PinholeCameraIntrinsic(get_intrinsic_matrix(color_frame)) + + # Validate that both frames are valid + if not aligned_depth_frame or not color_frame: + continue + + depth_image = Image(np.array(aligned_depth_frame.get_data())) + color_temp = np.asarray(color_frame.get_data()) + color_image = Image(color_temp) + + rgbd_image = create_rgbd_image_from_color_and_depth( + color_image, depth_image, depth_scale=1.0/depth_scale, + depth_trunc=clipping_distance_in_meters, + convert_rgb_to_intensity = False) + temp = create_point_cloud_from_rgbd_image( + rgbd_image, intrinsic) + temp.transform(flip_transform) + pcd.points = temp.points + pcd.colors = temp.colors + + if frame_count == 0: + vis.add_geometry(pcd) + + vis.update_geometry() + vis.poll_events() + vis.update_renderer() + + process_time = datetime.now() - dt0 + print("FPS: "+str(1/process_time.total_seconds())) + frame_count += 1 + + finally: + pipeline.stop() + vis.destroy_window() diff --git a/examples/Python/ReconstructionSystem/recorder_realsense.py b/examples/Python/ReconstructionSystem/sensors/realsense_recorder.py similarity index 98% rename from examples/Python/ReconstructionSystem/recorder_realsense.py rename to examples/Python/ReconstructionSystem/sensors/realsense_recorder.py index 37c63b7a506..c7d98d76a0a 100644 --- a/examples/Python/ReconstructionSystem/recorder_realsense.py +++ b/examples/Python/ReconstructionSystem/sensors/realsense_recorder.py @@ -48,7 +48,7 @@ def save_intrinsic_as_json(filename, frame): parser = argparse.ArgumentParser( description="Realsense Recorder. Please select one of the optional arguments") - parser.add_argument("--output_folder", default='dataset/realsense/', + parser.add_argument("--output_folder", default='../dataset/realsense/', help="set output folder") parser.add_argument("--record_rosbag", action='store_true', help="Recording rgbd stream into realsense.bag") diff --git a/src/Core/Geometry/PointCloudFactory.cpp b/src/Core/Geometry/PointCloudFactory.cpp index ece58426eb5..b077f27786a 100644 --- a/src/Core/Geometry/PointCloudFactory.cpp +++ b/src/Core/Geometry/PointCloudFactory.cpp @@ -36,6 +36,19 @@ namespace open3d{ namespace { +int CountValidDepthPixels(const Image &depth, int stride) +{ + int num_valid_pixels = 0; + for (int i = 0; i < depth.height_; i += stride) { + for (int j = 0; j < depth.width_; j += stride) { + const float *p = PointerAt(depth, j, i); + if (*p > 0) + num_valid_pixels += 1; + } + } + return num_valid_pixels; +} + std::shared_ptr CreatePointCloudFromFloatDepthImage( const Image &depth, const PinholeCameraIntrinsic &intrinsic, const Eigen::Matrix4d &extrinsic, int stride) @@ -44,6 +57,9 @@ std::shared_ptr CreatePointCloudFromFloatDepthImage( Eigen::Matrix4d camera_pose = extrinsic.inverse(); auto focal_length = intrinsic.GetFocalLength(); auto principal_point = intrinsic.GetPrincipalPoint(); + int num_valid_pixels = CountValidDepthPixels(depth, stride); + pointcloud->points_.resize(num_valid_pixels); + int cnt = 0; for (int i = 0; i < depth.height_; i += stride) { for (int j = 0; j < depth.width_; j += stride) { const float *p = PointerAt(depth, j, i); @@ -55,7 +71,7 @@ std::shared_ptr CreatePointCloudFromFloatDepthImage( focal_length.second; Eigen::Vector4d point = camera_pose * Eigen::Vector4d(x, y, z, 1.0); - pointcloud->points_.push_back(point.block<3, 1>(0, 0)); + pointcloud->points_[cnt++] = point.block<3, 1>(0, 0); } } } @@ -72,6 +88,10 @@ std::shared_ptr CreatePointCloudFromRGBDImageT( auto focal_length = intrinsic.GetFocalLength(); auto principal_point = intrinsic.GetPrincipalPoint(); double scale = (sizeof(TC) == 1) ? 255.0 : 1.0; + int num_valid_pixels = CountValidDepthPixels(image.depth_, 1); + pointcloud->points_.resize(num_valid_pixels); + pointcloud->colors_.resize(num_valid_pixels); + int cnt = 0; for (int i = 0; i < image.depth_.height_; i++) { float *p = (float *)(image.depth_.data_.data() + i * image.depth_.BytesPerLine()); @@ -86,9 +106,9 @@ std::shared_ptr CreatePointCloudFromRGBDImageT( focal_length.second; Eigen::Vector4d point = camera_pose * Eigen::Vector4d(x, y, z, 1.0); - pointcloud->points_.push_back(point.block<3, 1>(0, 0)); - pointcloud->colors_.push_back(Eigen::Vector3d( - pc[0], pc[(NC - 1) / 2], pc[NC - 1]) / scale); + pointcloud->points_[cnt] = point.block<3, 1>(0, 0); + pointcloud->colors_[cnt++] = Eigen::Vector3d( + pc[0], pc[(NC - 1) / 2], pc[NC - 1]) / scale; } } }