Add regi to node3

launch/run_main.launch

@@ -5,7 +5,7 @@
    <!--================================== Parameters ======================================== -->
 
     <!-- Number of poses the Baxter will move to for taking picture -->
-    <param name="num_goalposes" type="int" value="1" /> 
+    <param name="num_goalposes" type="int" value="2" /> 
 
     <!-- The topic for receiving PointCloud2 from Kinect -->
     <param name="topic_name_kinect_cloud" value="/kinect2/qhd/points" /> 

src/README.md

@@ -1,5 +1,4 @@
 
-Here stores the definition of cpp and python functions.  
-* The cpp are in "./my_xxx/" folders.  
-* The python are at root.  
+Here stores the definition of cpp functions.  
+
 

src_python/lib_cloud_registration.py

@@ -0,0 +1,94 @@
+import numpy as np
+import copy
+from open3d import *
+
+
+def drawTwoClouds(source, target, transformation):
+    source_temp = copy.deepcopy(source)
+    source_temp.transform(transformation)
+    draw_geometries([source_temp, target])
+
+
+def combineTwoClouds(cloud1, cloud2, radius_downsample=0.005):
+    cloud1_points = np.asarray(cloud1.points)
+    cloud2_points = np.asarray(cloud2.points)
+    cloud1_colors = np.asarray(cloud1.colors)
+    cloud2_colors = np.asarray(cloud2.colors)
+
+    out_points = np.vstack((cloud1_points, cloud2_points))
+    out_colors = np.vstack((cloud1_colors, cloud2_colors))
+
+    result = PointCloud()
+    result.points = Vector3dVector(out_points)
+    result.colors = Vector3dVector(out_colors)
+
+    result = voxel_down_sample(result, radius_downsample)
+    return result
+
+
+def registerClouds(src, target, radius_base=0.01):
+    # -- Use colored ICP to register src onto dst, and return the combined cloud
+    # This function is mainly copied from here.
+    # http://www.open3d.org/docs/tutorial/Advanced/colored_pointcloud_registration.html
+
+    # -- Params
+    ICP_distance_threshold = radius_base*4
+    voxel_radiuses = [radius_base*8, radius_base*2, radius_base]
+    max_iters = [50, 30, 14]
+
+    # -- Point to plane ICP
+    current_transformation = np.identity(4)
+    radius = radius_base
+    src_down = voxel_down_sample(src, radius)
+    target_down = voxel_down_sample(target, radius)
+    estimate_normals(src, KDTreeSearchParamHybrid(
+            radius=radius * 2, max_nn=30))
+    estimate_normals(target, KDTreeSearchParamHybrid(
+            radius=radius * 2, max_nn=30))
+    result_icp = registration_icp(src, target, ICP_distance_threshold,
+        current_transformation, TransformationEstimationPointToPlane())
+
+    if 0:
+        print("ICP result:")
+        print(result_icp)
+        drawTwoClouds(
+            src, target, result_icp.transformation)
+
+    # -- Colored pointcloud registration
+
+    current_transformation = np.identity(4)
+    print("\nStart Colored point cloud registration")
+    for ith_loop in range(len(voxel_radiuses)):
+        # Set param in this loop
+        max_iter = max_iters[ith_loop]
+        radius = voxel_radiuses[ith_loop]
+
+        # Downsample
+        src_down = voxel_down_sample(src, radius)
+        target_down = voxel_down_sample(target, radius)
+
+        # Estimate normal
+        estimate_normals(src_down, KDTreeSearchParamHybrid(
+            radius=radius * 2, max_nn=30))
+        estimate_normals(target_down, KDTreeSearchParamHybrid(
+            radius=radius * 2, max_nn=30))
+
+        # Applying colored point cloud registration
+        result_icp = registration_colored_icp(src_down, target_down,
+                                              radius, current_transformation,
+                                              ICPConvergenceCriteria(relative_fitness=1e-6,
+                                                                     relative_rmse=1e-6, max_iteration=max_iter))
+        current_transformation = result_icp.transformation
+
+        if 1:
+            print("\n{}th loop: radius={:.4f}, max_iter={}".format(
+                ith_loop, radius, max_iter))
+            print(result_icp)
+
+    # Transform src to target's frame
+    src_tmp = copy.deepcopy(src)
+    src_tmp.transform(result_icp.transformation)
+
+    # Combine the two
+    result_cloud = combineTwoClouds(src_tmp, target, radius_base)
+    return result_cloud, result_icp.transformation

src_ros/n1_work_flow_control.py

@@ -4,6 +4,7 @@
 import open3d
 import numpy as np
 import sys, os
+import cv2
 PYTHON_FILE_PATH=os.path.join(os.path.dirname(__file__))+"/"
 
 import rospy
@@ -13,7 +14,7 @@
 from tf.transformations import euler_from_quaternion, quaternion_from_euler, euler_matrix
 
 # Include my lib
-sys.path.append(PYTHON_FILE_PATH + "../src_ros")
+sys.path.append(PYTHON_FILE_PATH + "../src_python")
 from lib_cloud_conversion_between_Open3D_and_ROS import convertCloudFromOpen3dToRos
 from lib_geo_trans import form_T, quaternion_to_SO3
 from scan3d_by_baxter.msg import T4x4
@@ -63,6 +64,8 @@ def getCloudSize(open3d_cloud):
 # -- Main
 if __name__ == "__main__":
     rospy.init_node('node1')
+    print("Waiting for keypress to start ...")
+    cv2.waitKey(0)
 
     # Param settings
     topic_endeffector_pos = rospy.get_param("topic_n1_to_n2")

src_ros/n3_register_clouds_to_object.py

@@ -5,16 +5,17 @@
 import numpy as np
 import open3d
 import sys, os, copy
+from collections import deque
 PYTHON_FILE_PATH = os.path.join(os.path.dirname(__file__))+"/"
 
 # Include ROS
 import rospy
 from sensor_msgs.msg import PointCloud2
 
 # Include my lib
-sys.path.append(PYTHON_FILE_PATH + "../src_ros")
+sys.path.append(PYTHON_FILE_PATH + "../src_python")
 from lib_cloud_conversion_between_Open3D_and_ROS import convertCloudFromOpen3dToRos, convertCloudFromRosToOpen3d
-
+from lib_cloud_registration import drawTwoClouds, registerClouds
 
 # ---------------------------- functions ----------------------------
 def copyOpen3dCloud(src, dst):
@@ -31,8 +32,8 @@ def copyOpen3dCloud(src, dst):
     num_goalposes = rospy.get_param("num_goalposes")  # DEBUG, NOT USED NOW
 
     # -- Set subscriber
-    global received_ros_cloud
-    received_ros_cloud = None
+    global received_ros_clouds
+    received_ros_cloud = list()
 
     def callback(ros_cloud):
         global received_ros_cloud
@@ -63,10 +64,14 @@ def callback(ros_cloud):
             new_cloud_piece = convertCloudFromRosToOpen3d(received_ros_cloud)
 
             # Register Point Cloud
-            final_cloud = new_cloud_piece  # DEBUG, change to registration
+            if cnt==1:
+                copyOpen3dCloud(src=new_cloud_piece, dst=final_cloud)
+            else:
+                final_cloud, transformation = registerClouds(
+                    src=new_cloud_piece, target=final_cloud, radius_base=0.01)
 
             # Update point cloud
-            copyOpen3dCloud(final_cloud, vis_cloud)
+            copyOpen3dCloud(src=final_cloud, dst=vis_cloud)
             vis.add_geometry(vis_cloud)
             vis.update_geometry()
 

test/open3d_test_colored_ICP.py

@@ -4,101 +4,33 @@
 import copy
 from open3d import *
 import time
+import sys, os
+PYTHON_FILE_PATH=os.path.join(os.path.dirname(__file__))+"/"
 
-def draw_registration_result_original_color(src, target, transformation):
-    src_temp = copy.deepcopy(src)
-    src_temp.transform(transformation)
-    draw_geometries([src_temp, target])
-
-def combineTwoClouds(cloud1, cloud2):
-    cloud1_points=np.asarray(cloud1.points)
-    cloud2_points=np.asarray(cloud2.points)
-    cloud1_colors=np.asarray(cloud1.colors)
-    cloud2_colors=np.asarray(cloud2.colors)
-
-    out_points=np.vstack((cloud1_points,cloud2_points))
-    out_colors=np.vstack((cloud1_colors,cloud2_colors))
-
-    result=PointCloud()
-    result.points = Vector3dVector(out_points)
-    result.colors = Vector3dVector(out_colors)
-
-    radius = 0.01
-    result = voxel_down_sample(result, radius)
-    return result
-
-
+# Include my lib
+print(PYTHON_FILE_PATH)
+sys.path.append(PYTHON_FILE_PATH + "../src_python")
+from lib_cloud_registration import drawTwoClouds, registerClouds
 
 if __name__ == "__main__":
 
     # -- Load data
     src = read_point_cloud("../data_debug/ColoredICP_0.pcd")
     target = read_point_cloud("../data_debug/ColoredICP_1.pcd")
 
-    # draw initial alignment
+    # -- Draw initial alignment
     if 0:
         current_transformation = np.identity(4)
         draw_registration_result_original_color(
-               src, target, current_transformation)
-
-    # This function is mainly copied from here.
-    # http://www.open3d.org/docs/tutorial/Advanced/colored_pointcloud_registration.html
+            src, target, current_transformation)
 
-    # -- Point to plane ICP
-    current_transformation = np.identity(4);
-    distance_threshold = 0.02
-    result_icp = registration_icp(src, target, distance_threshold,
-            current_transformation, TransformationEstimationPointToPlane())
-    if 0:
-        print("ICP result:")
-        print(result_icp)
-        draw_registration_result_original_color(src, target, result_icp.transformation)
-
-    # -- Colored pointcloud registration
-    voxel_radiuses = [ 0.04, 0.02, 0.01 ];
-    max_iters = [ 50, 30, 14 ];
-    current_transformation = np.identity(4)
-    print("\nStart Colored point cloud registration")
-    for ith_loop in range(3):
-        # Set param in this loop
-        max_iter = max_iters[ith_loop]
-        radius = voxel_radiuses[ith_loop]
-
-
-        # Downsample
-        src_down = voxel_down_sample(src, radius)
-        target_down = voxel_down_sample(target, radius)
-
-        # Estimate normal
-        estimate_normals(src_down, KDTreeSearchParamHybrid(
-                radius = radius * 2, max_nn = 30))
-        estimate_normals(target_down, KDTreeSearchParamHybrid(
-                radius = radius * 2, max_nn = 30))
-
-        # Applying colored point cloud registration
-        result_icp = registration_colored_icp(src_down, target_down,
-                radius, current_transformation,
-                ICPConvergenceCriteria(relative_fitness = 1e-6,
-                relative_rmse = 1e-6, max_iteration = max_iter))
-        current_transformation = result_icp.transformation
-
-        if 1:
-            print("\n{}th loop: radius={:.2f}, max_iter={}".format(
-                    ith_loop, radius, max_iter))
-            print(result_icp)
-
-    # Transform src to dst's frame
-    src_tmp = copy.deepcopy(src)
-    src_tmp.transform(result_icp.transformation)
+    # -- Register clouds
+    result_cloud, transformation = registerClouds(src, target)
 
-    # Combine the two
-    result_cloud = combineTwoClouds(src_tmp, target)
-
-    # Print and plot
-    print(src)
-    print(target)
+    # -- Print and plot
+    # print(src)
+    # print(target)
     print(result_cloud)
+    print(transformation)
     time.sleep(0.3)
     draw_geometries([result_cloud])
-
-

test_ros/read_cloud_and_pub_by_open3d.py

@@ -14,7 +14,7 @@
 from sensor_msgs.msg import PointCloud2
 
 # Include my lib
-sys.path.append(PYTHON_FILE_PATH + "../src_ros")
+sys.path.append(PYTHON_FILE_PATH + "../src_python")
 from lib_cloud_conversion_between_Open3D_and_ROS import convertCloudFromOpen3dToRos, convertCloudFromRosToOpen3d
 
 # Main

test_ros/sub_cloud_and_display_by_open3d.py

@@ -26,7 +26,7 @@ def copyOpen3dCloud(src, dst):
 from sensor_msgs.msg import PointCloud2
 
 # Include my lib
-sys.path.append(PYTHON_FILE_PATH + "../src_ros")
+sys.path.append(PYTHON_FILE_PATH + "../src_python")
 from lib_cloud_conversion_between_Open3D_and_ROS import convertCloudFromOpen3dToRos, convertCloudFromRosToOpen3d
 
 def copyOpen3dCloud(src, dst):