Merge pull request #327 from syncle/fast_global_registration

Fast global registration

docs/Makefile

@@ -17,4 +17,4 @@ help:
 # Catch-all target: route all unknown targets to Sphinx using the new
 # "make mode" option.  $(O) is meant as a shortcut for $(SPHINXOPTS).
 %: Makefile
-	@$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O)
+	@$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O)

docs/introduction.rst

@@ -8,12 +8,12 @@ Open3D is an open-source library that supports rapid development of software tha
 If you use Open3D in an academic project, please cite our paper:
 ::
 
-	@article{Zhou2018,
-		author    = {Qian-Yi Zhou and Jaesik Park and Vladlen Koltun},
-		title     = {{Open3D}: {A} Modern Library for {3D} Data Processing},
-		journal   = {arXiv:1801.09847},
-		year      = {2018},
-	}
+    @article{Zhou2018,
+        author    = {Qian-Yi Zhou and Jaesik Park and Vladlen Koltun},
+        title     = {{Open3D}: {A} Modern Library for {3D} Data Processing},
+        journal   = {arXiv:1801.09847},
+        year      = {2018},
+    }
 
 Core features
 ======================

docs/make.bat

@@ -5,7 +5,7 @@ pushd %~dp0
 REM Command file for Sphinx documentation
 
 if "%SPHINXBUILD%" == "" (
-	set SPHINXBUILD=sphinx-build
+    set SPHINXBUILD=sphinx-build
 )
 set SOURCEDIR=.
 set BUILDDIR=_build
@@ -15,15 +15,15 @@ if "%1" == "" goto help
 
 %SPHINXBUILD% >NUL 2>NUL
 if errorlevel 9009 (
-	echo.
-	echo.The 'sphinx-build' command was not found. Make sure you have Sphinx
-	echo.installed, then set the SPHINXBUILD environment variable to point
-	echo.to the full path of the 'sphinx-build' executable. Alternatively you
-	echo.may add the Sphinx directory to PATH.
-	echo.
-	echo.If you don't have Sphinx installed, grab it from
-	echo.http://sphinx-doc.org/
-	exit /b 1
+    echo.
+    echo.The 'sphinx-build' command was not found. Make sure you have Sphinx
+    echo.installed, then set the SPHINXBUILD environment variable to point
+    echo.to the full path of the 'sphinx-build' executable. Alternatively you
+    echo.may add the Sphinx directory to PATH.
+    echo.
+    echo.If you don't have Sphinx installed, grab it from
+    echo.http://sphinx-doc.org/
+    exit /b 1
 )
 
 %SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS%

docs/tutorial/Advanced/fast_global_registration.rst

@@ -0,0 +1,116 @@
+.. _fast_global_registration:
+
+Fast global registration
+-------------------------------------
+
+RANSAC based :ref:`global_registration` solution may take a long time due to countless model proposal and evaluation.
+[Zhou2016]_ introduced a faster approach that quickly optimizes line process weights of few correspondences.
+As there is no model proposal and evaluation involved for each iteration, the approach proposed in [Zhou2016]_ can save a lot of computational time.
+
+This script compares running time of RANSAC based :ref:`global_registration` and implementation of [Zhou2016]_.
+
+.. code-block:: python
+
+    # src/Python/Tutorial/Advanced/fast_global_registration.py
+
+    from py3d import *
+    from global_registration import *
+    import numpy as np
+    import copy
+
+    import time
+
+    def execute_fast_global_registration(source_down, target_down,
+            source_fpfh, target_fpfh, voxel_size):
+        distance_threshold = voxel_size * 0.5
+        print(":: Apply fast global registration with distance threshold %.3f" \
+                % distance_threshold)
+        result = registration_fast_based_on_feature_matching(
+                source_down, target_down, source_fpfh, target_fpfh,
+                FastGlobalRegistrationOption(
+                maximum_correspondence_distance = distance_threshold))
+        return result
+
+    if __name__ == "__main__":
+
+        voxel_size = 0.05 # means 5cm for the dataset
+        source, target, source_down, target_down, source_fpfh, target_fpfh = \
+                prepare_dataset(voxel_size)
+
+        start = time.time()
+        result_ransac = execute_global_registration(source_down, target_down,
+                source_fpfh, target_fpfh, voxel_size)
+        print(result_ransac)
+        print("Global registration took %.3f sec.\n" % (time.time() - start))
+        draw_registration_result(source_down, target_down,
+                result_ransac.transformation)
+
+        start = time.time()
+        result_fast = execute_fast_global_registration(source_down, target_down,
+                source_fpfh, target_fpfh, voxel_size)
+        print("Fast global registration took %.3f sec.\n" % (time.time() - start))
+        draw_registration_result(source_down, target_down,
+                result_fast.transformation)
+
+Input
+``````````````````````````````````````
+
+.. code-block:: python
+
+    voxel_size = 0.05 # means 5cm for the dataset
+    source, target, source_down, target_down, source_fpfh, target_fpfh = \
+            prepare_dataset(voxel_size)
+
+For the pair comparison, the script reuses ``prepare_dataset`` function defined in :ref:`global_registration`.
+It produces a pair of downsampled point clouds as well as FPFH features.
+
+Baseline
+``````````````````````````````````````
+
+.. code-block:: python
+
+    start = time.time()
+    result_ransac = execute_global_registration(source_down, target_down,
+            source_fpfh, target_fpfh, voxel_size)
+    print(result_ransac)
+    print("Global registration took %.3f sec.\n" % (time.time() - start))
+    draw_registration_result(source_down, target_down,
+            result_ransac.transformation)
+
+This script calls RANSAC based :ref:`global_registration` as a baseline. After registration it displays the followings.
+
+.. image:: ../../_static/Advanced/fast_global_registration/ransac.png
+    :width: 400px
+
+.. code-block:: shell
+
+    RANSAC based global registration took 2.538 sec.
+
+Fast global registration
+``````````````````````````````````````
+
+With the same input used for baseline, the next script calls the implementation of [Zhou2016]_.
+
+.. code-block:: python
+
+    # in execute_fast_global_registration function
+
+    distance_threshold = voxel_size * 0.5
+    print(":: Apply fast global registration with distance threshold %.3f" \
+            % distance_threshold)
+    result = registration_fast_based_on_feature_matching(
+            source_down, target_down, source_fpfh, target_fpfh,
+            FastGlobalRegistrationOption(
+            maximum_correspondence_distance = distance_threshold))
+
+This script displays the followings.
+
+.. image:: ../../_static/Advanced/fast_global_registration/fgr.png
+    :width: 400px
+
+.. code-block:: shell
+
+    Fast global registration took 0.193 sec.
+
+With proper configuration, the accuracy of fast global registration is even comparable with ICP.
+Please refer [Zhou2016]_ for more experimental results.

docs/tutorial/Advanced/global_registration.rst

@@ -21,53 +21,73 @@ Both :ref:`icp_registration` and :ref:`colored_point_registration` are known as
         source_temp.transform(transformation)
         draw_geometries([source_temp, target_temp])
 
-    if __name__ == "__main__":
-
-        print("1. Load two point clouds and disturb initial pose.")
+    def prepare_dataset(voxel_size):
+        print(":: Load two point clouds and disturb initial pose.")
         source = read_point_cloud("../../TestData/ICP/cloud_bin_0.pcd")
         target = read_point_cloud("../../TestData/ICP/cloud_bin_1.pcd")
-        trans_init = np.asarray([[0.0, 1.0, 0.0, 0.0],
+        trans_init = np.asarray([[0.0, 0.0, 1.0, 0.0],
                                 [1.0, 0.0, 0.0, 0.0],
-                                [0.0, 0.0, 1.0, 0.0],
+                                [0.0, 1.0, 0.0, 0.0],
                                 [0.0, 0.0, 0.0, 1.0]])
         source.transform(trans_init)
         draw_registration_result(source, target, np.identity(4))
 
-        print("2. Downsample with a voxel size 0.05.")
-        source_down = voxel_down_sample(source, 0.05)
-        target_down = voxel_down_sample(target, 0.05)
+        print(":: Downsample with a voxel size %.3f." % voxel_size)
+        source_down = voxel_down_sample(source, voxel_size)
+        target_down = voxel_down_sample(target, voxel_size)
 
-        print("3. Estimate normal with search radius 0.1.")
+        radius_normal = voxel_size * 2
+        print(":: Estimate normal with search radius %.3f." % radius_normal)
         estimate_normals(source_down, KDTreeSearchParamHybrid(
-                radius = 0.1, max_nn = 30))
+                radius = radius_normal, max_nn = 30))
         estimate_normals(target_down, KDTreeSearchParamHybrid(
-                radius = 0.1, max_nn = 30))
+                radius = radius_normal, max_nn = 30))
 
-        print("4. Compute FPFH feature with search radius 0.25")
+        radius_feature = voxel_size * 5
+        print(":: Compute FPFH feature with search radius %.3f." % radius_feature)
         source_fpfh = compute_fpfh_feature(source_down,
-                KDTreeSearchParamHybrid(radius = 0.25, max_nn = 100))
+                KDTreeSearchParamHybrid(radius = radius_feature, max_nn = 100))
         target_fpfh = compute_fpfh_feature(target_down,
-                KDTreeSearchParamHybrid(radius = 0.25, max_nn = 100))
-
-        print("5. RANSAC registration on downsampled point clouds.")
-        print("   Since the downsampling voxel size is 0.05, we use a liberal")
-        print("   distance threshold 0.075.")
-        result_ransac = registration_ransac_based_on_feature_matching(
+                KDTreeSearchParamHybrid(radius = radius_feature, max_nn = 100))
+        return source, target, source_down, target_down, source_fpfh, target_fpfh
+
+    def execute_global_registration(
+            source_down, target_down, source_fpfh, target_fpfh, voxel_size):
+        distance_threshold = voxel_size * 1.5
+        print(":: RANSAC registration on downsampled point clouds.")
+        print("   Since the downsampling voxel size is %.3f," % voxel_size)
+        print("   we use a liberal distance threshold %.3f." % distance_threshold)
+        result = registration_ransac_based_on_feature_matching(
                 source_down, target_down, source_fpfh, target_fpfh, 0.075,
                 TransformationEstimationPointToPoint(False), 4,
                 [CorrespondenceCheckerBasedOnEdgeLength(0.9),
                 CorrespondenceCheckerBasedOnDistance(0.075)],
                 RANSACConvergenceCriteria(4000000, 500))
-        print(result_ransac)
-        draw_registration_result(source_down, target_down,
-                result_ransac.transformation)
+        return result
 
-        print("6. Point-to-plane ICP registration is applied on original point")
+    def refine_registration(source, target, source_fpfh, target_fpfh, voxel_size):
+        distance_threshold = voxel_size * 0.4
+        print(":: Point-to-plane ICP registration is applied on original point")
         print("   clouds to refine the alignment. This time we use a strict")
-        print("   distance threshold 0.02.")
-        result_icp = registration_icp(source, target, 0.02,
+        print("   distance threshold %.3f." % distance_threshold)
+        result = registration_icp(source, target, distance_threshold,
                 result_ransac.transformation,
                 TransformationEstimationPointToPlane())
+        return result
+
+    if __name__ == "__main__":
+        voxel_size = 0.05 # means 5cm for the dataset
+        source, target, source_down, target_down, source_fpfh, target_fpfh = \
+                prepare_dataset(voxel_size)
+
+        result_ransac = execute_global_registration(source_down, target_down,
+                source_fpfh, target_fpfh, voxel_size)
+        print(result_ransac)
+        draw_registration_result(source_down, target_down,
+                result_ransac.transformation)
+
+        result_icp = refine_registration(source, target,
+                source_fpfh, target_fpfh, voxel_size)
         print(result_icp)
         draw_registration_result(source, target, result_icp.transformation)
 
@@ -76,12 +96,14 @@ Input
 
 .. code-block:: python
 
-    print("1. Load two point clouds and disturb initial pose.")
+    # in prepare_dataset function
+
+    print(":: Load two point clouds and disturb initial pose.")
     source = read_point_cloud("../../TestData/ICP/cloud_bin_0.pcd")
     target = read_point_cloud("../../TestData/ICP/cloud_bin_1.pcd")
-    trans_init = np.asarray([[0.0, 1.0, 0.0, 0.0],
+    trans_init = np.asarray([[0.0, 0.0, 1.0, 0.0],
                             [1.0, 0.0, 0.0, 0.0],
-                            [0.0, 0.0, 1.0, 0.0],
+                            [0.0, 1.0, 0.0, 0.0],
                             [0.0, 0.0, 0.0, 1.0]])
     source.transform(trans_init)
     draw_registration_result(source, target, np.identity(4))
@@ -98,21 +120,25 @@ Extract geometric feature
 
 .. code-block:: python
 
-    print("2. Downsample with a voxel size 0.05.")
-    source_down = voxel_down_sample(source, 0.05)
-    target_down = voxel_down_sample(target, 0.05)
+    # in prepare_dataset function
 
-    print("3. Estimate normal with search radius 0.1.")
+    print(":: Downsample with a voxel size %.3f." % voxel_size)
+    source_down = voxel_down_sample(source, voxel_size)
+    target_down = voxel_down_sample(target, voxel_size)
+
+    radius_normal = voxel_size * 2
+    print(":: Estimate normal with search radius %.3f." % radius_normal)
     estimate_normals(source_down, KDTreeSearchParamHybrid(
-            radius = 0.1, max_nn = 30))
+            radius = radius_normal, max_nn = 30))
     estimate_normals(target_down, KDTreeSearchParamHybrid(
-            radius = 0.1, max_nn = 30))
+            radius = radius_normal, max_nn = 30))
 
-    print("4. Compute FPFH feature with search radius 0.25")
+    radius_feature = voxel_size * 5
+    print(":: Compute FPFH feature with search radius %.3f." % radius_feature)
     source_fpfh = compute_fpfh_feature(source_down,
-            KDTreeSearchParamHybrid(radius = 0.25, max_nn = 100))
+            KDTreeSearchParamHybrid(radius = radius_feature, max_nn = 100))
     target_fpfh = compute_fpfh_feature(target_down,
-            KDTreeSearchParamHybrid(radius = 0.25, max_nn = 100))
+            KDTreeSearchParamHybrid(radius = radius_feature, max_nn = 100))
 
 We down sample the point cloud, estimate normals, then compute a FPFH feature for each point. The FPFH feature is a 33-dimensional vector that describes the local geometric property of a point. A nearest neighbor query in the 33-dimensinal space can return points with similar local geometric structures. See [Rasu2009]_ for details.
 
@@ -123,20 +149,18 @@ RANSAC
 
 .. code-block:: python
 
-    print("5. RANSAC registration on down-sampled point clouds.")
-    print("   Since the downsampling voxel size is 0.05, we use a liberal")
-    print("   distance threshold 0.075.")
-    result_ransac = registration_ransac_based_on_feature_matching(
-            source_down, target_down, source_fpfh, target_fpfh,
-            fpfh, max_correspondence_distance = 0.075,
-            TransformationEstimationPointToPoint(False),
-            ransac_n = 4,
+    # in execute_global_registration function
+
+    distance_threshold = voxel_size * 1.5
+    print(":: RANSAC registration on downsampled point clouds.")
+    print("   Since the downsampling voxel size is %.3f," % voxel_size)
+    print("   we use a liberal distance threshold %.3f." % distance_threshold)
+    result = registration_ransac_based_on_feature_matching(
+            source_down, target_down, source_fpfh, target_fpfh, 0.075,
+            TransformationEstimationPointToPoint(False), 4,
             [CorrespondenceCheckerBasedOnEdgeLength(0.9),
             CorrespondenceCheckerBasedOnDistance(0.075)],
-            RANSACConvergenceCriteria(max_iteration = 4000000, max_validation = 500))
-    print(result_ransac)
-    draw_registration_result(source_down, target_down,
-            result_ransac.transformation)
+            RANSACConvergenceCriteria(4000000, 500))
 
 We use RANSAC for global registration. In each RANSAC iteration, ``ransac_n`` random points are picked from the source point cloud. Their corresponding points in the target point cloud are detected by querying the nearest neighbor in the 33-dimensional FPFH feature space. A pruning step takes fast pruning algorithms  to quickly reject false matches early.
 
@@ -153,6 +177,8 @@ We set the RANSAC parameters based on the empirical value provided by [Choi2015]
 .. image:: ../../_static/Advanced/global_registration/ransac.png
     :width: 400px
 
+.. Note:: Open3D provides faster implementation for global registration. Please refer :ref:`fast_global_registration`.
+
 .. _local_refinement:
 
 Local refinement
@@ -162,14 +188,15 @@ For performance reason, the global registration is only performed on a heavily d
 
 .. code-block:: python
 
-    print("6. Point-to-plane ICP registration is applied on original point")
+    # in refine_registration function
+
+    distance_threshold = voxel_size * 0.4
+    print(":: Point-to-plane ICP registration is applied on original point")
     print("   clouds to refine the alignment. This time we use a strict")
-    print("   distance threshold 0.02.")
-    result_icp = registration_icp(source, target, 0.02,
+    print("   distance threshold %.3f." % distance_threshold)
+    result = registration_icp(source, target, distance_threshold,
             result_ransac.transformation,
             TransformationEstimationPointToPlane())
-    print(result_icp)
-    draw_registration_result(source, target, result_icp.transformation)
 
 Outputs a tight alignment. This summarizes a complete pairwise registration workflow.
 

docs/tutorial/Advanced/index.rst

@@ -5,6 +5,7 @@ Advanced
 
     colored_pointcloud_registration
     global_registration
+    fast_global_registration
     multiway_registration
     rgbd_integration
     customized_visualization