adding sensors/realsense_pcd_visualizer.py & polish documents

docs/tutorial/ReconstructionSystem/capture_your_own_dataset.rst

@@ -3,7 +3,7 @@
 Capture your own dataset
 -------------------------------------
 
-If you have a `RealSense camera <https://www.intel.com/content/www/us/en/architecture-and-technology/realsense-overview.html>`_, capturing RGBD frames is easy by using ``recoder_realsense.py``.
+If you have a `RealSense camera <https://www.intel.com/content/www/us/en/architecture-and-technology/realsense-overview.html>`_, 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 <https://github.com/IntelRealSense/librealsense/tree/master/wrappers/python>`_ 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.
 

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()

examples/Python/ReconstructionSystem/recorder_realsense.py → 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")

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<float>(depth, j, i);
+            if (*p > 0)
+                num_valid_pixels += 1;
+        }
+    }
+    return num_valid_pixels;
+}
+
 std::shared_ptr<PointCloud> CreatePointCloudFromFloatDepthImage(
         const Image &depth, const PinholeCameraIntrinsic &intrinsic,
         const Eigen::Matrix4d &extrinsic, int stride)
@@ -44,6 +57,9 @@ std::shared_ptr<PointCloud> 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<float>(depth, j, i);
@@ -55,7 +71,7 @@ std::shared_ptr<PointCloud> 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<PointCloud> 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<PointCloud> 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;
             }
         }
     }