fix visualization/draw ICP example and add warnings (#6933)

* fix mismatch of selection sets, no target points in set and overall mismatch in number of correspondences.
---------
Co-authored-by: Sameer Sheorey <[email protected]>

CHANGELOG.md

@@ -45,6 +45,7 @@
 -   Fix minimal oriented bounding box of MeshBase derived classes and add new unit tests (PR #6898)
 -   Fix projection of point cloud to Depth/RGBD image if no position attribute is provided (PR #6880)
 -   Support lowercase types when reading PCD files (PR #6930)
+-   Fix visualization/draw ICP example and add warnings (PR #6933)
 
 ## 0.13
 

examples/python/visualization/draw.py

@@ -11,6 +11,7 @@
 import open3d.visualization as vis
 import os
 import random
+import warnings
 
 pyexample_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
 test_data_path = os.path.join(os.path.dirname(pyexample_path), 'test_data')
@@ -142,30 +143,46 @@ def selections():
     source_name = "Source (yellow)"
     target_name = "Target (blue)"
 
-    def do_icp_one_set(o3dvis):
+    def _prep_correspondences(o3dvis, two_set=False):
         # sets: [name: [{ "index": int, "order": int, "point": (x, y, z)}, ...],
         #        ...]
         sets = o3dvis.get_selection_sets()
-        source_picked = sorted(list(sets[0][source_name]),
-                               key=lambda x: x.order)
-        target_picked = sorted(list(sets[0][target_name]),
-                               key=lambda x: x.order)
-        source_indices = [idx.index for idx in source_picked]
-        target_indices = [idx.index for idx in target_picked]
-
-        t = get_icp_transform(source, target, source_indices, target_indices)
-        source.transform(t)
-
-        # Update the source geometry
-        o3dvis.remove_geometry(source_name)
-        o3dvis.add_geometry({"name": source_name, "geometry": source})
+        if not sets:
+            warnings.warn(
+                "Empty selection sets. Select point correspondences for initial rough transform.",
+                RuntimeWarning)
+            return [], []
+        if source_name not in sets[0]:
+            warnings.warn(
+                "First selection set should contain Source (yellow) points.",
+                RuntimeWarning)
+            return [], []
 
-    def do_icp_two_sets(o3dvis):
-        sets = o3dvis.get_selection_sets()
         source_set = sets[0][source_name]
-        target_set = sets[1][target_name]
+        if two_set:
+            if not len(sets) == 2:
+                warnings.warn(
+                    "Two set registration requires exactly two selection sets of corresponding points.",
+                    RuntimeWarning)
+                return [], []
+            target_set = sets[1][target_name]
+        else:
+            if target_name not in sets[0]:
+                warnings.warn(
+                    "Selection set should contain Target (blue) points.",
+                    RuntimeWarning)
+                return [], []
+            target_set = sets[0][target_name]
         source_picked = sorted(list(source_set), key=lambda x: x.order)
         target_picked = sorted(list(target_set), key=lambda x: x.order)
+        if len(source_picked) != len(target_picked):
+            warnings.warn(
+                f"Registration requires equal number of corresponding points (current selection: {len(source_picked)} source, {len(target_picked)} target).",
+                RuntimeWarning)
+            return [], []
+        return source_picked, target_picked
+
+    def _do_icp(o3dvis, source_picked, target_picked):
         source_indices = [idx.index for idx in source_picked]
         target_indices = [idx.index for idx in target_picked]
 
@@ -176,6 +193,12 @@ def do_icp_two_sets(o3dvis):
         o3dvis.remove_geometry(source_name)
         o3dvis.add_geometry({"name": source_name, "geometry": source})
 
+    def do_icp_one_set(o3dvis):
+        _do_icp(o3dvis, *_prep_correspondences(o3dvis))
+
+    def do_icp_two_sets(o3dvis):
+        _do_icp(o3dvis, *_prep_correspondences(o3dvis, two_set=True))
+
     vis.draw([{
         "name": source_name,
         "geometry": source